一维卡尔曼滤波器的Matlab仿真

卡尔曼滤波器的推导过程实在是太虐心了，花点时间和心思推推还是能推出来的。卡尔曼滤波器总结下来就是五大核心公式，这五个公式具体包括以下内容：

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。

一维卡尔曼滤波器的Matlab仿真

猜你喜欢