023卡尔曼滤波方程的推导

  博主参考严恭敏老师的讲义进行学习卡尔曼滤波，在讲义的基础上加以详细推导与说明。

  首先给出随机系统状态空间模型（注意该模型没有给出控制部分）：
$$\{\begin{array}{l}{X}_k={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\text{\hspace{1em}(1)}$$

注意：k/k-1表示从时刻k-1到k时刻的一步转移。

---

1、k-1时刻

目标：求解k-1时刻真值与状态估计的协方差

  真值：$X_{k-1}$
  状态估计：${\hat{X}}_{k-1}$ ，我们求得的状态估计基本不会等于真值，所以我们的状态估计就是最后求得的、可以用的状态；
  状态估计误差（为了便于区分，在此用e）：
$$\begin{gathered} e_{k-1}=X_{k-1}-{\hat{X}}_{k-1} \\ \text{\hspace{1em}(2)} \end{gathered}$$
  真值与k-1时刻状态估计的协方差：$P_{k-1}$
$$P_{k-1}=E[e_{k-1}{e}_{k-1}^T]=E[(X_{k-1}-{\hat{X}}_{k-1})（X_{k-1}-{\hat{X}}_{k-1}{）}^T]\text{\hspace{1em}(3)}$$

---

2、由k-1时刻预测k时刻状态

目标：求解真值与一步预测状态的协方差

  对k时刻的状态作预测：
$${\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}\text{\hspace{1em}(4)}$$
  零均值白噪声不会影响预测。另外注意${\hat{X}}_{k/k-1}$还不是最终的系统状态，说白了现在预测的这个状态还是不能用的，必须经过后面的修正得到${\hat{X}}_k$才能用，这也是他们下标的区别所在了。
  预测误差：
$$\begin{gathered} e_{k/k-1}=X_k-{\hat{X}}_{k/k-1} \\ \text{\hspace{1em}(5)} \end{gathered}$$
  真值与一步预测状态的协方差：$P_{k/k-1}$
$$\begin{array}{rl}{P}_{k/k-1}& =E[e_{k/k-1}{e}_{k/k-1}^T]\\ & =E[(X_k-{\hat{X}}_{k/k-1})(X_{k-1}-{\hat{X}}_{k/k-1}{)}^T]\end{array}\text{\hspace{1em}(6)}$$
  由公式(1)和公式(4)，并结合公式(2)可知：
$$\begin{array}{rl}{X}_k-{\hat{X}}_{k/k-1}& ={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}-{\Phi }_{k/k-1}{\hat{X}}_{k-1}\\ & ={\Phi }_{k/k-1}{e}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}\end{array}$$

注意：是$e_{k-1}$而不是$e_{k/k-1}$

  所以：
$$\begin{array}{rl}(X_k-{\hat{X}}_{k/k-1})(X_{k-1}-{\hat{X}}_{k/k-1}{)}^T& =({\Phi }_{k/k-1}{e}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1})({\Phi }_{k/k-1}{e}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}{)}^T\\ & =({\Phi }_{k/k-1}{e}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1})(e_{k-1}^T{\Phi }_{k/k-1}^T+W_{k-1}^T{\Gamma }_{k/k-1}^T)\\ & ={\Phi }_{k/k-1}{e}_{k-1}{e}_{k-1}^T{\Phi }_{k/k-1}^T+{\Phi }_{k/k-1}{e}_{k-1}{W}_{k-1}^T{\Gamma }_{k/k-1}^T\\ & +{\Gamma }_{k/k-1}{W}_{k-1}{e}_{k-1}^T{\Phi }_{k/k-1}^T+{\Gamma }_{k/k-1}{W}_{k-1}{W}_{k-1}^T{\Gamma }_{k/k-1}^T\end{array}$$

偷偷地把下标去掉再看一看：
$$\begin{array}{rl}(X_k-{\hat{X}}_{k/k-1})(X_{k-1}-{\hat{X}}_{k/k-1}{)}^T& =(\Phi e+\Gamma W)(\Phi e+\Gamma W{)}^T\\ & =(\Phi e+\Gamma W)(e^T{\Phi }^T+W^T{\Gamma }^T)\\ & =\Phi e{e}^T{\Phi }^T+\Phi e{W}^T{\Gamma }^T+\Gamma W{e}^T{\Phi }^T+\Gamma W{W}^T{\Gamma }^T\end{array}$$

  再考虑公式(3),并设噪声W的协方差阵为Q,这样公式(6)便有：

$$\begin{array}{rl}{P}_{k/k-1}& ={\Phi }_{k/k-1}E[e_{k-1}{e}_{k-1}^T]{\Phi }_{k/k-1}^T+{\Gamma }_{k/k-1}E[W_{k-1}{W}_{k-1}^T]{\Gamma }_{k/k-1}^T\\ & ={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k/k-1}{Q}_{k-1}{\Gamma }_{k/k-1}^T\end{array}\text{\hspace{1em}(7)}$$

注意：不相关的量期望为0

---

3、量测方程修正状态估计

目标：建立状态修正方程

  利用公式(1)-2中的量测方程k-1时刻预测k时刻的量测向量：
$${\hat{Z}}_{k/k-1}=H_k{\hat{X}}_{k/k-1}\text{\hspace{1em}(8)}$$
  相对于量测值，预测有误差：
$${\tilde{Z}}_{k/k-1}=Z_k-{\hat{Z}}_{k/k-1}\text{\hspace{1em}(9)}$$

  构造：
$${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k{\tilde{Z}}_{k/k-1}$$

注意：${\tilde{Z}}_{k/k-1}$是误差

  将公式(9)代入上式，并结合公式(4)和公式(8)可得状态修正方程：
$$\begin{array}{rl}{\hat{X}}_k& ={\hat{X}}_{k/k-1}+K_k(Z_k-{\hat{Z}}_{k/k-1})\\ & ={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\\ & =(I-K_k{H}_k){\hat{X}}_{k/k-1}+K_k{Z}_k\\ & =(I-K_k{H}_k){\Phi }_{k/k-1}{\hat{X}}_{k-1}+K_k{Z}_k\end{array}\text{\hspace{1em}(10)}$$

---

4、k时刻

目标：求解k时刻真值与状态估计的协方差

  状态方程k时刻的状态误差为：
$$e_k=X_k-{\hat{X}}_k$$
  将公式(10)第二个等号之后项代入上式并结合公式(5)和公式(1)-2可得：
$$\begin{array}{rl}{e}_k& =X_k-[{\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})]\\ & =e_{k/k-1}-K_k[Z_k-H_k{\hat{X}}_{k/k-1}]\\ & =e_{k/k-1}-K_k(H_k{X}_k+V_k-H_k{\hat{X}}_{k/k-1})\\ & =e_{k/k-1}-K_k[(H_k{X}_k-H_k{\hat{X}}_{k/k-1})+V_k]\\ & =e_{k/k-1}-K_k{H}_k{e}_{k/k-1}-K_k{V}_k\\ & =(I-K_k{H}_k)e_{k/k-1}-K_k{V}_k\end{array}\text{\hspace{1em}(11)}$$
  结合公式(6)并设噪声V的协方差阵为R，k时刻真值与状态估计的协方差阵为：
(12) P k = E [ e k e k T ] = E { [ ( I − K k H k ) e k / k − 1 − K k V k ] [ ( I − K k H k ) e k / k − 1 − K k V k ] T } = ( I − K k H k ) E [ e k / k − 1 e k / k − 1 T ] ( I − K k H k ) T + K k E [ V k V k T ] K k T = ( I − K k H k ) P k / k − 1 ( I − K k H k ) T + K k R k K k T \tag{12} \begin{aligned} P_k &= E[e_{k}e_{k}^T]\\ &= E \lbrace{ [(I - K_kH_k) e_{k/k-1} - K_k V_k] [(I - K_kH_k) e_{k/k-1} - K_k V_k]^T \rbrace}\\ &= (I - K_kH_k) E[e_{k/k-1} e_{k/k-1}^T] (I - K_kH_k)^T + K_k E[V_k V_k^T] K_k^T\\ &= (I - K_kH_k) P_{k/k-1} (I - K_kH_k)^T + K_k R_k K_k^T\\ \end{aligned} Pk​​=E[ek​ekT​]=E{[(I−Kk​Hk​)ek/k−1​−Kk​Vk​][(I−Kk​Hk​)ek/k−1​−Kk​Vk​]T}=(I−Kk​Hk​)E[ek/k−1​ek/k−1T​](I−Kk​Hk​)T+Kk​E[Vk​VkT​]KkT​=(I−Kk​Hk​)Pk/k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​​(12)

---

5、增益矩阵$K_k$

目标：求解增益矩阵$K_k$

  将公式(12)展开为：
$$\begin{array}{rl}{P}_k& =(I-K_k{H}_k)P_{k/k-1}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T\\ & =P_{k/k-1}-K_k{H}_k{P}_{k/k-1}-(K_k{H}_k{P}_{k/k-1}{)}^T\\ & +K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)K_k^T\end{array}\text{\hspace{1em}(13)}$$
  两边求迹运算：
$$tr(P_k)=tr(P_{k/k-1})-tr(K_k{H}_k{P}_{k/k-1})-tr((K_k{H}_k{P}_{k/k-1}{)}^T)+tr(K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)K_k^T)$$
  将上式看作关于K的一元二次函数，求极小值：
$$\begin{array}{rl}\frac{d}{d{K}_k}[tr(P_k)]& =0-(H_k{P}_{k/k-1}{)}^T-(H_k{P}_{k/k-1}{)}^T+2K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)\\ & =2[K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)-P_{k/k-1}{H}_k^T]\\ & =0\end{array}$$
  所以：
$$P_{k/k-1}{H}_k^T=K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)$$
  增益矩阵即为：
$$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(14)}$$

---

6、总结

目标：汇总卡尔曼滤波计算公式

  将公式(14)代入公式(13)可得：
$$\begin{array}{rl}{P}_k& =P_{k/k-1}-K_k{H}_k{P}_{k/k-1}-(K_k{H}_k{P}_{k/k-1}{)}^T\\ & +K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)K_k^T\\ & =P_{k/k-1}-K_k{H}_k{P}_{k/k-1}-P_{k/k-1}^T{H}_k^T{K}_k^T+P_{k/k-1}{H}_k^T{K}_k^T\\ & =P_{k/k-1}-K_k{H}_k{P}_{k/k-1}\\ & =(I-K_k{H}_k)P_{k/k-1}\end{array}$$

注：$P_{k/k-1}^T$=$P_{k/k-1}$

综上所述，卡尔曼滤波计算公式：

①状态一步预测：
$${\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}$$
②状态一步预测协方差阵：
$$P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k/k-1}{Q}_{k-1}{\Gamma }_{k/k-1}^T$$
③增益矩阵：
$$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}$$
④状态估计：
$${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$$
⑤状态估计协方差阵：
$$P_k=(I-K_k{H}_k)P_{k/k-1}$$

---

7、示例

Z = (1:100);
noise = randn(1, 100);
Z = Z +noise;

X = [0; 0];                %状态
P = [1 0; 0 1];            %状态协方差矩阵
F = [1 1; 0 1];            %状态转移矩阵
Q = [0.0001 0; 0 0.0001];  %状态转移协方差矩阵
H = [1 0];                 %观测矩阵
R = 1;

figure;
hold on;

for i = 1:100
    X_ = F*X;
    P_ = F*P*F' + Q;
    K = P_*H'/(H*P_*H' + R);
    X = X_ + K*( Z(i) - H*X );
    P = ( eye(2) - K*H ) * P_;
    
    plot(X(1), X(2), '.');  %横轴表示位置，纵轴表示速度
end

---

资源：一个轻松理解卡尔曼滤波的教学视频
卡尔曼滤波的原理以及在MATLAB中的实现-资源汇总页中下载