卡尔曼滤波
来源:互联网 发布:nginx http 307 编辑:程序博客网 时间:2024/05/16 06:03
原理部分就不过多介绍了,可以参考其他博主的博客。
这是一个自己写的卡尔曼滤波函数,其中作图部分不是很完善(只能输出二个状态),可以根据自己的需要进行修改,请务必注明出处,谢谢!如有错误,请大家指正。
%MATLAB%=====================================================================function Kalman_By_Vincent(x,x1,A,F,H,w,v,Q,R,P)% Kalman 滤波的实现% x是初始化的状态向量;% x1是初始化的预测的状态向量;一般为0向量,或者与x一致% A是系统矩阵;% H是观测阵;% F是状态误差阵;% w,v分别是状态噪声和观察噪声;% Q和R分别是w和v的方差;% P是滤波误差方差阵,一般初始化P{1}=eye(Dim);% Author:小秦Vincent;% Date:2015年12月12日。Dim =length(x{1}); %状态维度N =size(w,2); %噪声的个数%% 状态信号和观测信号,用来模拟实际的状态和观测for k=1:N-1 x{k+1}=A*x{k}+F*w(k); endfor k=1:N y(k) =H*x{k}+v(k);end%% Kalman迭代for k=2:N %% 预测 x1_pre =A*x1{k-1}; %状态预测值 y1_pre =H*x1_pre; %测量预测值 p1{k-1} =A*P{k-1}*A'+F*Q*F'; %预报误差方差阵更新 %% 更新 y1_NewMeg =y(k)-y1_pre; %新息 S =H*p1{k-1}*H'+R; %新息方差阵 K{k-1} =p1{k-1}*H'*inv(S); %增益矩阵更新 P{k} =(eye(Dim)-K{k-1}*H)*p1{k-1}; %滤波误差方差阵更新 x1{k} =x1_pre+K{k-1}*y1_NewMeg; %状态预测更新 end%cell数据矩阵化new_x =cell2mat(x);new_p1 =cell2mat(p1); %预报方差阵 new_K =cell2mat(K); %增益矩阵new_P =cell2mat(P); %滤波方差阵new_x1 =cell2mat(x1); %预测%% 预测数据y1=H*new_x1;%% 显示输出图像figure(3)set(gcf,'position',[400 150 800 600]);plot(y,'b','linewidth',2);grid on;hold on;plot(y1,'r');legend('原始观测信号','Kalman输出观测信号');% title('原始信号与预测信号示意图','fontsize',15);hold off;figure(4)%状态量1set(gcf,'position',[400 150 800 600]);plot(new_x(1,:),'b','linewidth',2);grid on;hold on;plot(new_x1(1,:),'r','linestyle','-');legend('状态一','Kalman滤波输出状态一');hold off;figure(5)%状态量2set(gcf,'position',[400 150 800 600]);plot(new_x(2,:),'b','linewidth',2);grid on;hold on;plot(new_x1(2,:),'r','linestyle','-');legend('状态二','Kalman滤波输出状态二');hold off;figure(6)%增益set(gcf,'position',[400 150 800 600]);plot(new_K(1,:),'b','linewidth',2);grid on;hold on;plot(new_K(2,:),'g','linewidth',2);legend('状态一增益','状态二增益');hold off;%---------------------------------------------------------------------
2 0
- 扩展卡尔曼滤波+卡尔曼滤波
- 卡尔曼滤波1
- 卡尔曼滤波2
- 卡尔曼滤波
- 卡尔曼滤波简介
- 学习卡尔曼滤波
- 卡尔曼滤波简介
- kalman卡尔曼滤波
- 卡尔曼滤波相关
- 卡尔曼滤波
- 卡尔曼滤波详解
- 卡尔曼滤波
- 卡尔曼滤波简介
- 卡尔曼滤波随想
- 卡尔曼滤波
- 卡尔曼滤波简介
- 卡尔曼滤波学习
- 卡尔曼滤波原理
- C语言总结
- C# Enum,Int,String的互相转换
- Scala 学习笔记1(介绍)
- Vectors.dense()
- Congratulations for getting IEEE SPL cover picture
- 卡尔曼滤波
- 《AndroidStudio每日一贴》10. 选择查找结果
- Android MotionEvent传递流程 源码笔记
- translate xml参数讲解
- 黑马程序员--Java编程之常见对象
- Facebook开源《AsyncDisplayKit介绍》翻译
- [数据结构]双机调度问题
- vi编辑器技巧总结(更新中)
- Fragment的基本使用方法与通信