(参考文献)
1、 卡尔曼滤波分为估计和更新两个部分
2、分为过程噪声和测量噪声
4、理解这五个公式,
最重要的是kalman增益计算
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('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%
因为观测矩阵没有对速度进行观测,默认得到的速度值是真实值,经过卡尔曼滤波,速度的误差值也逐渐变小
ps:学习还要继续,如有理解错误的地方,请您指正