卡尔曼滤波器的推导过程实在是太虐心了,花点时间和心思推推还是能推出来的。卡尔曼滤波器总结下来就是五大核心公式,这五个公式具体包括以下内容:
1. 计算预测值;
2. 计算预测值和真实值之间误差的协方差矩阵;
3. 计算卡尔曼增益;
4. 根据预测值和测量值,计算估计值(滤波值);
5. 计算估计值和真实值之间误差的协方差矩阵。
一维卡尔曼滤波器的Matlab程序如下:
close all;
clear all;
%系统方程:X(k+1)=A*X(k)+w(k)
%观测方程:Z(k)=H*X(k)+v(k)
A=1;
H=1;
X(1)=25; %系统状态初始化
w=1*randn(100,1); %系统噪声
v=0.1*randn(100,1); %测量噪声
Q=cov(w);
R=cov(v);
Xe(1)=25; %状态估计初值
Pe(1)=0; %估计值与真实值之间的协方差矩阵初值
for i=2:100
X(i)=A*X(i-1)+w(i); %系统方程
Z(i)=H*X(i)+v(i); %观测方程
%卡尔曼滤波五大核心方程
Xp(i)=A*Xe(i-1); %根据估计值计算预测值
Pp(i)=A*Pe(i-1)*A'+Q; %计算预测值与真实值之间误差的协方差矩阵
K(i)=Pp(i)*H'/((H*Pp(i)*H'+R)); %计算卡尔曼增益
Xe(i)=Xp(i)+K(i)*(Z(i)-H*Xp(i));%计算估计值
Pe(i)=Pp(i)-K(i)*H*Pp(i); %计算估计值与真实值之间误差的协方差矩阵
end
n=1:100;
subplot(211)
plot(n,Xp,'r','linewidth',2); %绘制预测值
hold on;
plot(n,Z,'b','linewidth',2); %绘制测量值
hold on;
plot(n,Xe,'g','linewidth',2); %绘制估计值
hold on;
plot(n,X-Xe,'k','linewidth',2); %绘制误差
legend('预测值','观测值','滤波值','误差')
grid
subplot(212)
plot(n,K,'b','linewidth',2); %绘制卡尔曼增益
legend('卡尔曼增益')
grid
仿真结果如下图所示,不难发现,由于测量噪声相比于系统噪声小的多,所以估计值更倾向于观测值,此时的卡尔曼增益趋近于1。
如果将前述代码中的系统噪声和测量噪声改为如下形式:
w=0.1*randn(100,1); %系统噪声
v=1*randn(100,1); %测量噪声
这时的仿真结果如下图,由于此时的测量噪声相比于系统噪声要大的多,因而估计值更倾向于预测值,此时的卡尔曼增益不再趋近于1。