Kalman滤波算法

MATLAB实验

实验要求

一连续平稳的随机信号x(t)x(t) 满足方程：

x(t)=Ax(t−1)+w(k)x(t)=Ax(t-1)+w(k)

其中A=e−0.02A=e^{-0.02}，w(k)w(k) 为均值为00 方差为1−e−0.041-e^{-0.04} 的噪声，且观测值受加性噪声所干扰，即：

y(k)=x(k)+v(k)y(k)=x(k)+v(k)

其中v(k)v(k) 为均值为00 方差为11 的白噪声。

若测量的离散值已知，即：

1
Y=[-3.2 -0.8 -14 -16 -17 -18 -3.3 -2.4 -18 -0.3 -0.4 -0.8 -19 -2.0 -1.2 -11 -14 -0.9 0.8 10 0.2 0.5 2.4 -0.5 0.5 -13 0.5 10 -12 0.5 -0.6 -15 -0.7 15 0.5 -0.7 -2.0 -19 -17 -11 -14]

自编卡尔曼滤波递推程序，估计信号x(t)x(t) 的波形。

实验步骤

自写程序处理

通过阅读 Kalman滤波算法的原理，可以得到如下方程：

{x^k=Akx^k−1+Hk(yk−CkAkx^k−1)Hk=Pk′CkT(CkPk′CkT+Rk)−1Pk′=AkPk−1AkT+Qk−1Pk=(I−HkCk)Pk′\left\{\begin{array}{l} \hat{x}_{k}=A_{k} \hat{x}_{k-1}+H_{k}\left(y_{k}-C_{k} A_{k} \hat{x}_{k-1}\right) \\ H_{k}=P_{k}^{\prime} C_{k}^{T}\left(C_{k} P_{k}^{\prime} C_{k}^{T}+R_{k}\right)^{-1} \\ P_{k}^{\prime}=A_{k} P_{k-1} A_{k}^{T}+Q_{k-1} \\ P_{k}=\left(I-H_{k} C_{k}\right) P_{k}^{\prime} \end{array}\right.

这个方程揭示了 Kalman滤波 的逆推步骤：

  1. 给定初值x0,P0x_0,P_0 （本问题中，Ak,Qk,Ck,RkA_k,Q_k,C_k,R_k 不随时间变换，为常数）
  2. 利用初值计算x^0=Akx0,  P1′=AkP0AkT+Qk\hat x_0=A_kx_0,\;P'_1=A_kP_0A_k^T+Q_k
  3. 进而计算 Kalman增益H1=P1′CkT(CkP1′CkT+Rk)−1H_1=P_{1}^{\prime} C_{k}^{T}\left(C_{k} P_{1}^{\prime} C_{k}^{T}+R_{k}\right)^{-1}
  4. 利用增益估计出新的x^1=Akx^0+Hk(y1−CkAkx^0)\hat x_1=A_{k} \hat{x}_{0}+H_{k}\left(y_{1}-C_{k} A_{k} \hat{x}_{0}\right) 以及P1=(I−H1Ck)P0′P_1=(I-H_1C_k)P_0'
  5. 如此反复迭代实现 Prediction 和 Correction。

其中：

预测步即是xk=Akx^k,  Pk′=AkPk−1AkT+Qk−1x_k=A_k\hat x_k,\;P'_k=A_kP_{k-1}A_k^T+Q_{k-1}

更新步是Hk=Pk′CkT(CkPk′CkT+Rk)−1,  x^k=Akx^k−1+Hk(yk−CkAkx^k−1),  Pk=(I−HkCk)Pk′H_{k}=P_{k}^{\prime} C_{k}^{T}\left(C_{k} P_{k}^{\prime} C_{k}^{T}+R_{k}\right)^{-1},\;\hat{x}_{k}=A_{k} \hat{x}_{k-1}+H_{k}\left(y_{k}-C_{k} A_{k} \hat{x}_{k-1}\right),\;P_{k}=\left(I-H_{k} C_{k}\right) P_{k}^{\prime}

于是我们就可以完整地编写 Kalman滤波 的代码了：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
clear;clc; 
%基本参数值 
Ak = exp(-0.02);
Ck = 1; 
Qk = 1-exp(-0.04);
Rk = 1; 
%初始时刻的卡尔曼最优值设置 
X0 = 0;
P0 = 1; 
%观测值y(k) 
Y = [-3.2 -0.8 -14 -16 -17 -18 -3.3 -2.4 -18 -0.3 -0.4 -0.8 -19 -2.0 -1.2 ...
    -11 -14 -0.9 0.8 10 0.2 0.5 2.4 -0.5 0.5 -13 0.5 10 -12 0.5 -0.6 -15 -0.7 15 ... 
0.5 -0.7 -2.0 -19 -17 -11 -14]; 
%数据长度 
N = length(Y); 
for k = 1:N 
    if k == 1 %k=l时由初值开始计算 
        %预测
        X_pre(k) = Ak*X0;       
        P_pre(k) = Ak*P0*Ak+Qk;
        
        K(k) = P_pre(k)*Ck'*(Ck*P_pre(k)*Ck'+Rk)^(-1); %卡尔曼增益
        X_kalman(k) = Ak*X_pre(k)+K(k)*(Y(k)-Ck*Ak*X_pre(k));
        I = eye(size(K(k)));%生成单位矩阵
        P_kalman(k) = (I-K(k)*Ck)*P_pre(k);

    else %k>l时开始递推 
        %预测
        X_pre(k) = Ak*(X_kalman(k-1));
        P_pre(k) = Ak*P_kalman(k-1)*Ak+Qk;
        %更新
        K(k) = P_pre(k)*Ck'*(Ck*P_pre(k)*Ck'+Rk)^(-1);
        X_kalman(k) = Ak*X_pre(k)+K(k)*(Y(k)-Ck*Ak*X_pre(k));
        I = eye(size(K(k)));
        P_kalman(k) = (I-K(k)*Ck)*P_pre(k);
    end
end
M=1:N;
T=0.02*M;
%作图,画出x(t)的波形 
figure () 
plot(T,Y,'r','LineWidth',1);
hold on;
plot(T,X_kalman,'b','LineWidth',1); 
legend('测量信号y(t)','Kalman估计信号x(t)')