卡尔曼滤波（2）：一个简单的例子

为了说明离散性卡尔曼滤波器的用法，我将用一个最简单的例子来进行说明。

假设我们现在对一个房间内的温度进行测试，房间内温度初值为25°c，每过一个时间周期，温度x都将在上一个周期温度的基础上变动

于是我们可以建立状态差分方程

再假设我们手上有一个误差比较大的温度计（如果误差不大也不需要卡尔曼滤波了），通过温度计，我们可以得到测量值z，我们建立观测方程

每当经过一个时间周期，我们用上一周期得到的后验估计量来估计下一周期的先验估计量，可以得到时间更新方程

由该先验估计量估计先验协方差

由估计协方差我们可以计算大名鼎鼎的卡尔曼增益

由卡尔曼增益我们继续计算x在k时刻的后验估计量

计算后验协方差

重复以上五个步骤

以下是matlab程序仿真

function main
N=120;
CON=25;
Xexpect=CON*ones(1,N);
X=zeros(1,N);
Xkf=zeros(1,N);
Z=zeros(1,N);
P=zeros(1,N);

%赋初值
X(1)=25.1;
P(1)=0.01;
Z(1)=24.9;
Xkf(1)=Z(1);

%噪声
Q=0.01;
R=0.25;
W=sqrt(Q)*randn(1,N);   %方差决定噪声大小
V=sqrt(R)*randn(1,N);   %方差决定噪声大小

%系统矩阵
F=1;
G=1;
H=1;
I=eye(1);   %本系统状态为一维


%模拟房间温度及测量过程，并滤波
for k=2:N
    %第一步，随时间推移，房间真实温度波动变化
    %k时刻的真实温度，温度计是不知道的，但是它又是真实存在的，因此我们要过滤杂波，尽量收敛到真实温度
    X(k)=F*X(k-1)+G*W(k-1);
    
    %第二步，随着时间推移，获取实时数据
    %温度计对k时刻房间温度的测量，Kalman滤波是站在温度计角度进行的。
    %他不知道此刻真实温度，只能站在本次测量值和上一次估计值来做处理。其目的是最大限度降低测量噪声R的影响。尽可能的逼近X（k);
    Z(k)=H*X(k)+V(k);
    
    %第三步，卡尔曼滤波。
    %有了k时刻的观测和k-1时刻的状态就可以滤波了。
    X_pre=F*Xkf(k-1);      %状态预测
    P_pre=F*P(k-1)*F'+Q;    %协方差预测
    Kg=P_pre*inv(H*P_pre*H'+R); %计算卡尔曼增益
    e=Z(k)-H*X_pre;     %新息
    Xkf(k)=X_pre+Kg*e;  %状态更新
    P(k)=(I-Kg*H)*P_pre;    %协方差更新
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%计算误差
Err_Messure=zeros(1,N); %测量值与真实值之间的偏差
Err_Kalman=zeros(1,N);  %估计值与真实值之间的偏差
for k=1:N
    Err_Messure=abs(Z(k)-X(k));
    Err_Kalman=abs(Xkf(k)-X(k));
end



t=1:N;
figure; %画图显示
%依次输出理论值，叠加过程噪声的真实值
%温度计测量值，卡尔曼估计值
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');
%plot(t,X,'-r',t,Xkf,'-g*');
legend('期望值','真实值','观测值','卡尔曼滤波值');
%legend('真实值','卡尔曼滤波值');
xlabel('采样时间/s');
ylabel('温度值/c');

%误差分析图
figure %画图显示
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k*');
legend('测量偏差','卡尔曼滤波偏差');
xlabel('采样时间/s');
ylabel('温度值/c');