卡尔曼滤波

原理部分就不过多介绍了，可以参考其他博主的博客。
这是一个自己写的卡尔曼滤波函数，其中作图部分不是很完善（只能输出二个状态），可以根据自己的需要进行修改，请务必注明出处，谢谢！如有错误，请大家指正。

%=====================================================================
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:Vincentqin；
% 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);   
end
for 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)%状态量1
set(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)%状态量2
set(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;

%---------------------------------------------------------------------