kalman滤波学习入门向总结

kalman滤波学习总结

1 学习总结

（参考文献）
1、 卡尔曼滤波分为估计和更新两个部分
2、分为过程噪声和测量噪声

• 过程噪声：表示状态运动过程中产生的，比如下落过程的空气阻力、加热水壶时的热量散失，总之这一部分噪声不是由于观测器问题引起，并且要是满足高斯正态分布。Q表示其方差
• 测量噪声：这一部分噪声由于测量器件引起，比如陀螺仪不准啥的，R表示其方差
• 两个噪声都是满足高斯正态分布随机数

3、仔细理解下图

• 使用上一时刻的最优估计，估计出当前时刻的值 （但是这个值不一定准确）
• 使用当前时刻观测值（这个也不一定准确）
• 所以，结合了当前的观测值和和当前时刻的估计值，得到这一时刻的最优估计

4、理解这五个公式，
最重要的是kalman增益计算

2 演示代码

clear;
clc;

N = 1000; %1000个采样点
T = 10;   %表示10s的仿真时间

%噪声
Q = [0,0;0,0];  %过程噪声方差为0，下落过程忽略空气阻力
R = 1;
W=sqrt(Q)*randn(2,N);%既然Q为0，即W=0
V=sqrt(R)*randn(1,N);%测量噪声V(k) 即为measure noise  用于产生高斯正态分布随机数

%系数矩阵
t = 10/1000;
F = [1,t;0,1];
B = [0.5*t*t;t];
U = 9.8;
H = [1,0];

%初始化
X = zeros(2,N);  %用于记录下落的位移和下落的速度
X(:,1) = [0,0];  %以自由落体的方式下落
P = [1,0;0,0.5];

Z = zeros(1,N);  %用于记录观测器的观测值
Z(1) = H*X(:,1);

Xkf = zeros(2,N);
Xkf(:,1)=X(:,1);

err_P=zeros(2,N);
err_P(1,1)=P(1,1);
err_P(2,1)=P(2,2);
I=eye(2); %二维系统

%%%%%%%%%%%%%
%kalman滤波需要
%1、利用上一时刻对当前时刻的预测值
%2、这一时刻的观测值
for k=2:N
    
    X(:,k)=F*X(:,k-1)+B*U+W(k);%物体自由下落
    Z(k)=H*X(:,k)+V(k);        %位移传感器对目标进行观测,但是位移传感器有测量的误差
    
    %%%卡尔曼滤波%%%
    %预测
    X_pre=F*Xkf(:,k-1)+B*U     %状态预测   用上一时刻对这一时刻预测
    P_pre=F*P*F'+Q;            %协方差预测
    %最终用上一时刻的值先验估计对这一时刻进行估计
    %更新
    K=P_pre*H'*inv(H*P_pre*H'+R);    %计算卡尔曼增益
    Xkf(:,k)=X_pre+K*(Z(k)-H*X_pre); %状态更新
    P=(I-K*H)*P_pre;                %方差更新
    
    %误差均方值
    err_P(1,k)=P(1,1);
    err_P(2,k)=P(2,2);
end

%误差计算
measure_err_position=zeros(1,N);  %位移的测量误差
kalman_err_position=zeros(1,N);   %卡尔曼估计的位移与真实位移之间的偏差
kalman_err_velocity=zeros(1,N);   %卡尔曼估计的速度与真实速度之间的偏差
for k=1:N
    measure_err_position(k)=Z(k)-X(1,k);
    kalman_err_position(k)=Xkf(1,k)-X(1,k);
    kalman_err_velocity(k)=Xkf(2,k)-X(2,k);
end

%%%%%%%%%%%%%%%
%画图输出
%噪声图
figure
plot(V);
title('观测噪声')
%位置偏差
figure
hold on,box on;
plot(measure_err_position,'-r.');%测量的位移误差
plot(kalman_err_position,'-g.');%卡尔曼估计位置误差
legend('观测的位移误差','卡尔曼滤波的位移误差')
figure
hold on,box on;
plot(kalman_err_velocity);            %卡尔曼滤波下速度的误差
plot(zeros(1,N));
title('速度误差')
figure
plot(err_P(1,:));
title('位移误差均方值')
figure
plot(err_P(2,:));
title('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%

3 结果分析

显而易见卡尔曼滤波后的位移误差明显变小，而且最后收敛

因为观测矩阵没有对速度进行观测，默认得到的速度值是真实值，经过卡尔曼滤波，速度的误差值也逐渐变小

ps：学习还要继续，如有理解错误的地方，请您指正