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:学习还要继续,如有理解错误的地方,请您指正