卡尔曼滤波matlab示例

卡尔曼滤波matlab示例

% 卡尔曼滤波实验
clc;
clear all;
close all;
%==过程移噪声和观测噪声的估计是否加噪=======
noise_flag=0;%如果加噪赋值1，否则赋值0
Q_N=100;% 对过程噪声方差Q估计的噪声方差
R_N=100;
Q_N_Mean=0;
R_N_Mean=0;
%============================================
N=20;
% 过程噪声方差Q
Q = 0.5;
% 观测噪声方差R
R = 100;
H=1;% 观测矩阵
A=1;% 转移矩阵
B=1;% 控制矩阵
u=ones(1,20);% 控制量
x(1)=0;% 初始x
p(1)=0;
I=eye(1,1);
for ii = 1:N-1
    x(ii+1)=x(ii)*A+B*u(ii)+sqrt(Q)*randn(1,1);
    % 真实运动情况为
    % 转移增量（来自于上一次运动）+控制增量（来自外部控制）+噪声（过程噪声）
end
for ii=1:N
    z(ii)=H*x(ii)+sqrt(R)*randn(1,1);
    % 观测情况为
    % 观测量（来自此时刻观测值）+噪声（观测噪声）
end
x_hat_prior(1)=x(1);
for k=1:N-1
    if noise_flag==1
    noiseQ=sqrt(Q_N)*randn(1,1)+Q_N_Mean;
    noiseR=sqrt(R_N)*randn(1,1)+R_N_Mean;
    else 
    noiseQ=0；
    noiseR=0;
  	end
    x_hat_prior(k+1)= A*x_hat_prior(k)+B*u(k);
    p_prior(k+1)=A*p(k)*A'+(Q+noiseQ);
    K(k+1)=(p_prior(k+1)*H')/(H*p_prior(k+1)+(R+noiseR));
    x_hat(k+1)=x_hat_prior(k+1)+K(k+1)*(z(k+1)-H*x_hat_prior(k+1));
    p(k+1)=(I-K(k+1)*H)*p_prior(k+1);
end
image=figure(1);
subplot(2,1,1);
plot(1:N,x,'g');
hold on;
plot(1:N,z,'b-.');
plot(1:N,x_hat,'r--');
legend('真实值','观测值','估计值');
hold off;
e_z=abs(x-z);
e_h=abs(x-x_hat);
subplot(2,1,2);
plot(1:N,e_z,'r');
hold on;
plot(1:N,e_h,'b');
legend('观测误差','估计误差');
hold off;

卡尔曼滤波是针对多数据来源的一种取舍方法，根据对数据来源中噪声大小去分析更应该偏向于哪种消息来源。一种是观测值，一种是计算值，都有噪声的情况下需要先明确目前各自噪声大小。再由计算式得出理论计算得来的先验估计，以及通过后验初始误差的协方差和过程噪声协方差得到下一步的先验误差协方差；以上是预测部分，接下来是校正，计算卡尔曼增益以及后验估计，并且更新后验误差的协方差；以上讲的很笼统，具体推到推荐视频：Kalaman filter—DR_CAN_https://space.bilibili.com/230105574?spm_id_from=333.788.b_765f7570696e666f.2
虽然大多数讲卡尔曼的文章给出解释很生动，但是这个是唯一让人听懂，明白的。