| RMVL 2.4.0 Robotic Manipulation and Vision Library

载入中...
搜索中...
未找到
卡尔曼滤波

卡尔曼滤波详细公式推导

作者
  张华铨
  赵曦

日期
  2024/03/24

版本
  2.0

上一篇教程：基于 TOPSIS 模型的熵权法
下一篇教程：扩展卡尔曼滤波

相关类 rm::KalmanFilter

\[\def\red#1{\color{red}{#1}} \def\teal#1{\color{teal}{#1}} \def\green#1{\color{green}{#1}} \def\transparent#1{\color{transparent}{#1}} \def\orange#1{\color{orange}{#1}} \def\Var{\mathrm{Var}} \def\Cov{\mathrm{Cov}} \def\tr{\mathrm{tr}} \def\fml#1{\text{(#1)}} \]

## 1 卡尔曼滤波

### 1.1 简介

卡尔曼滤波器是一个 Optimal Recursive Data Processing Algorithm，即最优化递归数字处理算法，与其说滤波器，倒不如说这是一个 观测器 。简单来说，卡尔曼滤波器能够估计目标的当前位置，并结合观测结果，以最高置信度获得当前位置。

### 1.2 初尝递归算法

示例

我们需要测量一个硬币的长度 \(x\)，现测出了如下结果

\[\begin{align}z_1&=30.2\mathrm{mm}\\z_2&=29.7\mathrm{mm}\\z_3&=30.1\mathrm{mm}\\&\vdots\end{align}\]

我们要利用这些观测结果来估计硬币的真实长度，很自然的我们可以想到使用取平均的方式，我们可以利用 \(k\) 个观测值，求出硬币长度在第 \(k\) 次的估计值 \(\hat x_k\)。

\[\hat x_k=\frac{z_1+z_2+\cdots+z_k}k\tag{1-1}\]

这是一条非常简单的取算数平均的公式，但是我们为了估计硬币的长度，需要用到所有的观测值，例如我们已经测了 5 次硬币的长度，并且使用公式 \(\fml{1-1}\) 得到了第 5 次的平均值（长度的估计值），在测完第 6 次长度准备计算第 6 次估计值的时候，使用公式 \(\fml{1-1}\) 还需要重新使用前 5 次的观测值。当观测次数非常高的时候，计算的压力就逐渐高起来了。

针对这一问题，我们可以改写公式 \(\fml{1-1}\)

\[\begin{align}\hat x_k&=\frac1k(z_1+z_2+\cdots+z_k)\\ &=\frac1k(z_1+z_2+\cdots+z_{k-1})+\frac1kz_k\\ &=\frac{k-1}k·\frac1{\red{k-1}}\red{(z_1+z_2+\cdots+z_{k-1})}+\frac1kz_k\\ &=\frac{k-1}k\red{\hat x_{k-1}}-\frac1kz_k\\&=\hat x_{k-1}-\frac1k\hat x_{k-1}+\frac1kz_k\\ &=\hat x_{k-1}+\frac1k(z_k-\hat x_{k-1})\tag{1-2a}\end{align}\]

这样我们就把硬币长度的估计值，改写成由上一次估计值和当前观测值共同作用的形式。并且我们发现，随着测量次数 \(k\) 增大， \(\frac1k\) 趋向于 \(0\)， \(\hat x_k\) 趋向于 \(\hat x_{k-1}\)，这也就是说，随着 \(k\) 增长，测量结果将不再重要。

为了不失一般性，我们把公式 \(\fml{1-2a}\) 的结果改写成以下形式。

\[\boxed{\hat x_k=\hat x_{k-1}+\red{K_k}(z_k-\hat x_{k-1})}\tag{1-2b}\]

其中 \(K_k\) 表示 Kalman Gain，即卡尔曼增益。对于 \(K_k\)，我们先给出他的计算结果。

\[K_k=\frac{e_{{EST}_{k-1}}}{e_{{EST}_{k-1}}+e_{{MEA}_k}}\tag{1-3}\]

其中， \(e_{{EST}_{k-1}}\) 表示第 \(k-1\) 次的估计误差， \(e_{{MEA}_k}\) 表示第 \(k\) 次的测量误差。可以得到，在 \(k\) 时刻，

1. 若 \(e_{{EST}_{k-1}}\gg e_{{MEA}_k}\)，则 \(K_k\to1\)， \(\hat x_k=\hat x_{k-1}+1\times(z_k-\hat x_{k-1})=z_k\)
2. 若 \(e_{{EST}_{k-1}}\ll e_{{MEA}_k}\)，则 \(K_k\to0\)， \(\hat x_k=\hat x_{k-1}+0\times(z_k-\hat x_{k-1})=\hat x_{k-1}\)

### 1.3 数据融合

现在我们可以使用公式 \(\fml{1-2b}\) 的思想来研究数据融合。如果有一辆车以恒定速度行驶，现在得到了两个数据：

- 根据匀速公式计算得到的汽车当前位置（算出来的，记作 \(\teal{x_1}\)）
- 汽车的当前距离传感器数据（测出来的，记作 \(\red{x_2}\)）

图1-1 汽车运动估计

由于自身的原因，它的位置并不是由匀速运动公式得到的精确位置，它的距离传感器数据也不完全准确。两者都有一定的误差。那么我们现在如何估计汽车的实际位置呢？

我们使用公式 \(\fml{1-2b}\) 的思想，得到估计值

\[\hat x=\teal{x_1}+\green{K_k}(\red{x_2}-\teal{x_1})\tag{1-4}\]

我们的目标很明确，希望求出这个 \(\green{K_k}\)，使得估计值 \(\hat x\) 是最优的，即 \(\hat x\) 的方差 \(\Var(\hat x)\) 最小，即 \(\hat x\to x_{实际值}\)。

不妨令 \(\left\{\begin{align}\teal{x_1=30\mathrm m\qquad\sigma_1=2\mathrm m}\\\red{x_2=32\mathrm m\qquad\sigma_2=4\mathrm m}\end{align}\right.\)，则有

\[\begin{align}\Var(\hat x)&=\Var[x_1+K_k(x_2-x_1)]\\&=\Var(x_1-K_kx_1+K_kx_2)\\ &=\Var[(1-K_k)x_1+K_kx_2]\\由x_1和x_2相互独立\quad&=\Var[(1-k)x_1]+\Var(K_kx_2)\\ &=(1-K_k)^2\Var(x_1)+K_k^2\Var(x_2)\\ &=(1-K_k)^2\sigma_1^2+K_k^2\sigma_2^2\tag{1-5}\end{align}\]

要求 \(\Var(\hat x)\) 的最小值，只需令 \(\mathrm \Var(\hat x)'|_{K_k}=0\) 即可，即：

\[\frac{\mathrm d\Var(\hat x)}{\mathrm dK_k}=-2(1-K_k)\sigma_1^2+2K_k\sigma_2^2=0\tag{1-6}\]

即

\[K_k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}\tag{1-7}\]

代入数据后，有

\[\left\{\begin{align} K_k&=\frac{2^2}{2^2+4^2}=0.2\\ \hat x&=30+0.2(32-30)=30.4\mathrm m\quad\orange{理论最优解}\\ \sigma_{\hat x}^2&=(1-0.2)^22^2+0.2^24^2=3.2\quad(\sigma_{\hat x}=1.79) \end{align}\right.\]

### 1.4 协方差矩阵

数据 | \(x\) | \(y\) | \(z\)
--- | --- | --- | ---
\(1\) | \(x_1\) | \(y_1\) | \(z_1\)
\(2\) | \(x_2\) | \(y_2\) | \(z_2\)
\(3\) | \(x_3\) | \(y_3\) | \(z_3\)
平均值 | \(\bar x\) | \(\bar y\) | \(\bar z\)

方差

表明单个变量的波动程度

\[\begin{align} \Var(x)&=\frac13\left[(x_1-\bar x)^2+(x_2-\bar x)^2+(x_3-\bar x)^2\right]=\frac13\sum_{i=1}^3(x-\bar x)^2\stackrel{\triangle}{=}\sigma_x^2\\ \Var(y)&=\frac13\left[(y_1-\bar y)^2+(y_2-\bar y)^2+(y_3-\bar y)^2\right]=\frac13\sum_{i=1}^3(y-\bar y)^2\stackrel{\triangle}{=}\sigma_y^2\\ \Var(z)&=\frac13\left[(z_1-\bar z)^2+(z_2-\bar z)^2+(z_3-\bar z)^2\right]=\frac13\sum_{i=1}^3(z-\bar z)^2\stackrel{\triangle}{=}\sigma_z^2 \end{align}\]

一般的，方差具有以下推导公式

\[\begin{align}\Var(X)&=\frac1n\sum_{i=1}^n(X-\bar X)^2\\&=\frac1n\sum_{i=1}^n(X-EX)^2\\ 定义\quad&=\green{E\left[(X-EX)^2\right]}\\ &=E\left[X^2-2X·EX+(EX)^2\right]\\&=EX^2-2EX·EX+(EX)^2\\ &=\red{EX^2-(EX)^2}\tag{1-8}\end{align}\]

协方差

表明变量之间的相关性，为正数表示正相关，为负数表示负相关，绝对值越大表示相关性越强

\[\begin{align} \Cov(x,y)&=\frac13\left[(x_1-\bar x)(y_1-\bar y)+(x_2-\bar x)(y_2-\bar y)+(x_3-\bar x)(y_3-\bar y)\right]\\ &=\frac13\sum_{i=1}^3(x-\bar x)(y-\bar y)\stackrel{\triangle}{=}\sigma_x\sigma_y\\ \Cov(y,z)&=\frac13\left[(y_1-\bar y)(z_1-\bar z)+(y_2-\bar y)(z_2-\bar z)+(y_3-\bar y)(z_3-\bar z)\right]\\ &=\frac13\sum_{i=1}^3(y-\bar y)(z-\bar z)\stackrel{\triangle}{=}\sigma_y\sigma_z\\ \Cov(x,z)&=\frac13\left[(x_1-\bar x)(z_1-\bar z)+(x_2-\bar x)(z_2-\bar z)+(x_3-\bar x)(z_3-\bar z)\right]\\ &=\frac13\sum_{i=1}^3(x-\bar x)(z-\bar z)\stackrel{\triangle}{=}\sigma_x\sigma_z \end{align}\]

一般的，协方差具有以下推导公式

\[\begin{align}\Cov(X,Y) &=\frac1n\sum_{i=1}^n(X-\bar X)(Y-\bar Y)\\ &=\frac1n\sum_{i=1}^n(X-EX)(Y-EY)\\ 定义\quad&=\green{E\left[(X-EX)(Y-EY)\right]}\\ &=E(XY-X·EY-Y·EX+EX·EY)\\ &=E(XY)-EX·EY-EY·EX+EX·EY\\ &=\red{E(XY)-EXEY} \tag{1-9}\end{align}\]

协方差矩阵

按照协方差的定义出发，可以有

\[\begin{align}\Cov(x_i,x_j) &=E\left[(x_i-Ex_i)(x_j-Ex_j)\right]=\sigma_{x_i}\sigma_{x_j}\\ 令误差e_i=x_i-E(x_i)\quad&=\green{E(e_ie_j)} \tag{1-10a}\end{align}\]

若取 \(i,j=1,2,\cdots,n\)，则可以得到协方差矩阵 \(P\)，表示为

\[\begin{align}P&=\begin{bmatrix} \sigma_{x_1}^2&\sigma_{x_1}\sigma_{x_2}&\cdots&\sigma_{x_1}\sigma_{x_n}\\ \sigma_{x_2}\sigma_{x_1}&\sigma_{x_2}^2&\cdots&\sigma_{x_2}\sigma_{x_n}\\ \vdots&\vdots&\ddots&\vdots\\ \sigma_{x_n}\sigma_{x_1}&\sigma_{x_n}\sigma_{x_2}&\cdots&\sigma_{x_n}^2 \end{bmatrix}\\&=\begin{bmatrix}Ee_1^2&Ee_1Ee_2&\cdots&Ee_1Ee_n\\ Ee_2Ee_1&Ee_2^2&\cdots&Ee_2Ee_n\\\vdots&\vdots&\ddots&\vdots\\ Ee_nEe_1&Ee_nEe_2&\cdots&Ee_n^2\end{bmatrix}\\&= E\begin{bmatrix}e_1^2&e_1e_2&\cdots&e_1e_n\\ e_2e_1&e_2^2&\cdots&e_2e_n\\\vdots&\vdots&\ddots&\vdots\\ e_ne_1&e_ne_2&\cdots&e_n^2\end{bmatrix}=\green{E\left(\boldsymbol e\boldsymbol e^T\right)}\tag{1-10b}\end{align}\]

### 1.5 卡尔曼增益推导

对于一个系统，可以使用状态空间方程来描述其运动

\[\left\{\begin{align} \dot{\boldsymbol x}&=F\boldsymbol x+G\boldsymbol u\\ \boldsymbol z&=H\boldsymbol x \end{align}\right.\tag{1-11}\]

根据 3.4 方程组的 Runge-Kutta 公式 中精确解的公式 \(\text{(b)}\)，我们可以知道状态空间方程的解的表达式，即

\[\boldsymbol x(t)=e^{F(t-t_0)}\boldsymbol x(t_0)+\int_{t_0}^te^{F(t-\tau)}G\boldsymbol u(\tau)\mathrm d\tau\tag{1-12}\]

以 \(T\)为周期离散采样+零阶保持器，可以得到

\[\begin{align} \boldsymbol x(t_k)&=e^{F(t_k-t_{k-1})}\boldsymbol x(t_{k-1})+\int_{t_{k-1}}^{t_k}e^{F(t_k-\tau)}G\boldsymbol u(\tau)\mathrm d\tau\\ &=e^{FT}\boldsymbol x(t_{k-1})+\int_{t_{k-1}}^{t_k}e^{F(t_k-\tau)}\mathrm d\tau·G\boldsymbol u(t_{k-1})\\ &=e^{FT}\boldsymbol x(t_{k-1})+\left[-F^{-1}e^{F(t_k-\tau)}\right]_{t_{k-1}}^{t_k}·G\boldsymbol u(t_{k-1})\\ &=e^{FT}\boldsymbol x(t_{k-1})+F^{-1}(e^{FT}-I)G\boldsymbol u(t_{k-1})\\ \boldsymbol x_k&=\red{e^{FT}}\boldsymbol x_{k-1}+\green{F^{-1}(e^{FT}-I)G}\boldsymbol u_{k-1}\\ 简记为\quad\boldsymbol x_k&=\red A\boldsymbol x_{k-1}+\green B\boldsymbol u_{k-1}\tag{1-13a}\end{align}\]

因此我们可以得到离散系统的状态空间方程

\[\left\{\begin{align} \boldsymbol x_k&=A\boldsymbol x_{k-1}+B\boldsymbol u_{k-1}\\ \boldsymbol z_k&=H\boldsymbol x_k \end{align}\right.\tag{1-13b}\]

这其实是个不准确的结果，因为如果我们考虑上噪声，公式 \(\fml{1-13b}\) 应该改写为

\[\left\{\begin{align} \boldsymbol x_k&=A\boldsymbol x_{k-1}+B\boldsymbol u_{k-1}\red{+\boldsymbol w_{k-1}}&p(\boldsymbol w)\sim N(0,Q)\\ \boldsymbol z_k&=H\boldsymbol x_k\red{+\boldsymbol v_k}&p(\boldsymbol v)\sim N(0,R) \end{align}\right.\tag{1-13c}\]

- \(w_{k-1}\) 称为过程噪声，来自于算不准的部分，其中 \(Q\) 满足以下形式

  \[Q=E(\boldsymbol w\boldsymbol w^T)=\begin{bmatrix}\sigma_{w_1}^2&\sigma_{w_1}\sigma_{w_2}&\cdots&\sigma_{w_1}\sigma_{w_n}\\ \sigma_{w_2}\sigma_{w_1}&\sigma_{w_2}^2&\cdots&\sigma_{w_2}\sigma_{w_n}\\ \vdots&\vdots&\ddots&\vdots\\\sigma_{w_n}\sigma_{w_1}&\sigma_{w_n}\sigma_{w_2}&\cdots&\sigma_{w_n}^2\end{bmatrix}\]

  称为过程噪声协方差矩阵
- \(v_{k-1}\) 称为测量噪声，来自于测不准的部分，其中 \(R\) 满足以下形式

  \[R=E(\boldsymbol v\boldsymbol v^T)=\begin{bmatrix}\sigma_{v_1}^2&\sigma_{v_1}\sigma_{v_2}&\cdots&\sigma_{v_1}\sigma_{v_n}\\ \sigma_{v_2}\sigma_{v_1}&\sigma_{v_2}^2&\cdots&\sigma_{v_2}\sigma_{v_n}\\ \vdots&\vdots&\ddots&\vdots\\\sigma_{v_n}\sigma_{v_1}&\sigma_{v_n}\sigma_{v_2}&\cdots&\sigma_{v_n}^2\end{bmatrix}\]

  称为测量噪声协方差矩阵

但是，这两个误差我们无从得知，我们只能使用公式 \(\fml{1-13b}\) 的形式进行近似估计，通过 \(\boldsymbol x_k=A\boldsymbol x_{k-1}+B\boldsymbol u_{k-1}\) 算出来的 \(\boldsymbol x_k\) 称为先验状态估计，一般写为

\[\red{\hat{\boldsymbol x}_k^-=A\boldsymbol x_{k-1}+B\boldsymbol u_{k-1}\tag{1-14}}\]

通过 \(\boldsymbol z_k=H\boldsymbol x_k\) 得到的 \(\boldsymbol x_k\) 是测出来的结果，记为 \(\hat{\boldsymbol x}_{k_{MEA}}\)，即

\[\hat{\boldsymbol x}_{k_{MEA}}=H^+\boldsymbol z_k\tag{1-15}\]

因为 \(H^{-1}\) 可能不存在，这里先使用 M-P 广义逆 \(H^+\) 来表示。

注解
  对于一个线性方程组

  \[A\boldsymbol x=\boldsymbol b\]

  必定存在最小二乘解

  \[\boldsymbol x=(A^TA)^{-1}A^T\boldsymbol b\]

  可以令 \(A^+=(A^TA)^{-1}A^T\) 来表示 Moore-Penrose 广义逆，即

  \[\boldsymbol x=A^+\boldsymbol b\]

  当 \(A\) 可逆时， \(A^+=A^{-1}\).

目前的两个结果 \(\hat{\boldsymbol x}_k^-\) 和 \(\hat{\boldsymbol x}_{k_{MEA}}\) 都不准确，因此可以回顾 1.3 数据融合 的部分，在公式 \(\fml{1-4}\) 中使用了算出来的 \(\teal{x_1}\) 和测出来的 \(\red{x_2}\) 得到了最优估计值 \(\hat x\)，为此我们可以仿照这一步骤来求出离散系统状态的最优估计值 \(\hat{\boldsymbol x}_k\)，称为后验状态估计。

\[\begin{align}\hat{\boldsymbol x}_k&=\hat{\boldsymbol x}_k^-+\green{G_k}(\hat{\boldsymbol x}_{k_{MEA}}-\hat{\boldsymbol x}_k^-)\\ &=\hat{\boldsymbol x}_k^-+\green{G_k}(H^+\boldsymbol z_k-\hat{\boldsymbol x}_k^-)\tag{1-16}\end{align}\]

令 \(G_k=K_kH\)，可以得到

\[\red{\hat{\boldsymbol x}_k=\hat{\boldsymbol x}_k^-+K_k(\boldsymbol z_k-H\hat{\boldsymbol x}_k^-)\tag{1-17}}\]

我们的目标依然很明确，希望求出这个 \(\green{K_k}\)，使得后验状态估计值 \(\hat{\boldsymbol x}_k\) 是最优的，即 \(\hat{\boldsymbol x}_k\) 的方差 \(\Var(\hat{\boldsymbol x}_k)\) 最小，即 \(\hat{\boldsymbol x}_k\to \boldsymbol x_k\)。如何衡量 \(\hat{\boldsymbol x}_k\) 的方差 \(\Var(\hat{\boldsymbol x}_k)\)？我们可以令状态误差 \(\boldsymbol e_k=\boldsymbol x_k-\hat{\boldsymbol x}_k\)，那么此时 \(p(\boldsymbol e_k)\sim N(0,P_k)\)，其中 \(P_k\) 是误差协方差矩阵，并且满足

\[P_k=\begin{bmatrix}\sigma_{e_1}^2&\cdots&\sigma_{e_1}\sigma_{e_n}\\\vdots&&\vdots\\\sigma_{e_n}\sigma_{e_1}&\cdots&\sigma_{e_n}^2\end{bmatrix}\tag{1-18}\]

方差 \(\Var(\hat{\boldsymbol x}_k)\) 最小，即 \(\sum\limits_{i=1}^{n}\sigma_{e_i}^2\) 最小，即 \(P_k\) 的迹 \(\tr(P_k)\) 最小。

对于状态误差，有

\[\begin{align} \boldsymbol e_k&=\boldsymbol x_k-\hat{\boldsymbol x}_k=\boldsymbol x_k-\left(\hat{\boldsymbol x}_k^-+K_k(\boldsymbol z_k-H\hat{\boldsymbol x}_k^-)\right)\\ &=\boldsymbol x_k-\hat{\boldsymbol x}_k^--K_k\boldsymbol z_k+K_kH\hat{\boldsymbol x}_k^-\\ &=\boldsymbol x_k-\hat{\boldsymbol x}_k^--K_k(H\boldsymbol x_k+\boldsymbol v_k)+K_kH\hat{\boldsymbol x}_k^-\\ &=\boldsymbol x_k-\hat{\boldsymbol x}_k^--K_kH\boldsymbol x_k-K_k\boldsymbol v_k+K_kH\hat{\boldsymbol x}_k^-\\ &=(\boldsymbol x_k-\hat{\boldsymbol x}_k^-)-K_kH(\boldsymbol x_k-\hat{\boldsymbol x}_k^-)-K_k\boldsymbol v_k\\ &=(I-K_kH)\teal{(\boldsymbol x_k-\hat{\boldsymbol x}_k^-)}-K_k\boldsymbol v_k\\ &=(I-K_kH)\teal{\boldsymbol e_k^-}-K_k\boldsymbol v_k\tag{1-19}\end{align}\]

则误差协方差矩阵可以表示为

\[\begin{align}P_k &=E\left(\boldsymbol e_k\boldsymbol e_k^T\right)=E\left[(\boldsymbol x_k-\hat{\boldsymbol x}_k)(\boldsymbol x_k-\hat{\boldsymbol x}_k)^T\right]\\ &=E\left[\left((I-K_kH)\teal{\boldsymbol e_k^-}-K_k\boldsymbol v_k\right)\left((I-K_kH)\teal{\boldsymbol e_k^-}-K_k\boldsymbol v_k\right)^T\right]\\ &=E\left[\left(\green{(I-K_kH)\boldsymbol e_k^-}-\red{K_k\boldsymbol v_k}\right)\left(\teal{{\boldsymbol e_k^-}^T(I-K_kH)^T}-\orange{\boldsymbol v_k^TK_k^T}\right)\right]\\ &=E\left[\green{(I-K_kH)\boldsymbol e_k^-}\teal{{\boldsymbol e_k^-}^T(I-K_kH)^T}-\green{(I-K_kH)\boldsymbol e_k^-}\orange{\boldsymbol v_k^TK_k^T}-\right.\\ &\transparent=\left.\red{K_k\boldsymbol v_k}\teal{{\boldsymbol e_k^-}^T(I-K_kH)^T}+\red{K_k\boldsymbol v_k}\orange{\boldsymbol v_k^TK_k^T}\right]\\ &=\red{E(}(I-K_kH)\boldsymbol e_k^-{\boldsymbol e_k^-}^T(I-K_kH)^T\red{)}-\\ &\transparent=\red{E(}(I-K_kH)\boldsymbol e_k^-\boldsymbol v_k^TK_k^T\red{)}-\\ &\transparent=\red{E(}K_k\boldsymbol v_k{\boldsymbol e_k^-}^T(I-K_kH)^T\red{)}+\\ &\transparent=\red{E(}K_k\boldsymbol v_k\boldsymbol v_k^TK_k^T\red{)}\\ 提出常数的期望\quad&=(I-K_kH)\red{E(}\boldsymbol e_k^-{\boldsymbol e_k^-}^T\red{)}(I-K_kH)^T-\\ &\transparent=(I-K_kH)\red{E(}\boldsymbol e_k^-\boldsymbol v_k^T\red{)}K_k^T-\\ &\transparent=K_k\red{E(}\boldsymbol v_k{\boldsymbol e_k^-}^T\red{)}(I-K_kH)^T+\\ &\transparent=K_k\red{E(}\boldsymbol v_k\boldsymbol v_k^T\red{)}K_k^T\\ &=(I-K_kH)\red{P_k^-}(I-K_kH)^T+\\ \boldsymbol e_k^-和{\boldsymbol v_k}^T相互独立\quad&\transparent=0+\\ 且期望为0\quad&\transparent=0+\\ &\transparent=K_k\red{R}K_k^T\\ &=\left(\green{P_k^-}-\red{K_kHP_k^-}\right)\left(\teal I-\orange{H^TK_k^T}\right)+K_kRK_k^T\\ &=\green{P_k^-}-\red{K_kHP_k^-}-\green{P_k^-}\orange{H^TK_k^T}+\red{K_kHP_k^-}\orange{H^TK_k^T}+K_kRK_k^T\\ P_k^-对称\quad&=P_k^--K_kHP_k^--\left(K_kHP_k^-\right)^T+K_kHP_k^-H^TK_k^T+K_kRK_k^T \tag{1-20}\end{align}\]

因此， \(P_k\) 的迹可以表示为

\[\tr(P_k)=\tr(P_k^-)-2\tr(K_kHP_k^-)+\tr(K_kHP_k^-H^TK_k^T)+\tr(K_kRK_k^T)\tag{1-21}\]

希望 \(\tr(P_k)\) 有最小值，则需要对 \(K_k\) 求导，则有

\[\frac{\mathrm d\tr(P_k)}{\mathrm dK_k}=0-2\frac{\mathrm d\tr(K_kHP_k^-)}{\mathrm dK_k}+\frac{\mathrm d\tr(K_kHP_k^-H^TK_k^T)}{\mathrm dK_k}+\frac{\mathrm d\tr(K_kRK_k^T)}{\mathrm dK_k}=0\tag{1-22}\]

对于形如 \(\frac{\mathrm d\tr(AB)}{A}\) 的导数计算，可以通过拿一个 2 阶矩阵作为例子

> 令

> \[A=\begin{bmatrix}a_{11}&a_{12}\\a_{21}&a_{22}\end{bmatrix},\quad B=\begin{bmatrix}b_{11}&b_{12}\\b_{21}&b_{22}\end{bmatrix}\]

> 则

> \[AB=\begin{bmatrix}a_{11}b_{11}+a_{12}b_{21}&a_{11}b_{21}+a_{12}b_{22}\\a_{21}b_{11}+a_{22}b_{21}&a_{21}b_{12}+a_{22}b_{22}\end{bmatrix}\]

> 则

> \[\tr(AB)=a_{11}b_{11}+a_{12}b_{21}+a_{21}b_{12}+a_{22}b_{22}\]

> 则

> \[\frac{\mathrm d\tr(AB)}{\mathrm dA}=\begin{bmatrix}\frac{\partial\tr(AB)}{\partial a_{11}}&\frac{\partial\tr(AB)}{\partial a_{12}}\\\frac{\partial\tr(AB)}{\partial a_{21}}&\frac{\partial\tr(AB)}{\partial a_{22}}\end{bmatrix}=\begin{bmatrix}b_{11}&b_{21}\\b_{12}&b_{22}\end{bmatrix}=B^T\]

因此可以得到

\[\frac{\mathrm d\tr(AB)}{\mathrm dA}=B^T\tag{1-23a}\]

同理，我们也能验证如下结论

\[\begin{align}\frac{\mathrm d\tr(ABA^T)}{\mathrm dA}&=AB+AB^T\\ 当B对称时\quad&=2AB\end{align}\tag{1-23b}\]

那么，公式 \(\fml{1-22}\)可以写为

\[\begin{align}2\frac{\mathrm d\tr(K_kHP_k^-)}{\mathrm dK_k} &=\frac{\mathrm d\tr(K_kHP_k^-H^TK_k^T)}{\mathrm dK_k}+\frac{\mathrm d\tr(K_kRK_k^T)}{\mathrm dK_k}\\ 2{P_k^-}^TH^T&=2K_kHP_k^-H^T+2K_kR\\ P_k^-H^T&=K_k\left(HP_k^-H^T+R\right) \tag{1-24}\end{align}\]

最终可以得到误差协方差矩阵的迹最小时的卡尔曼增益 \(K_k\) 的表达式

\[\red{K_k=P_k^-H^T\left(HP_k^-H^T+R\right)^{-1}\tag{1-25}}\]

讨论

- 当 \(R\) 很大时， \(K_k\to0,\quad\hat{\boldsymbol x}_k=\hat{\boldsymbol x}_k^-+0(\boldsymbol z_k-H\hat{\boldsymbol x}_k^-)=\red{\hat{\boldsymbol x}_k^-}\)
- 当 \(R\) 很小时， \(K_k\to H^+,\quad\hat{\boldsymbol x}_k=\hat{\boldsymbol x}_k^-+H^+(\boldsymbol z_k-H\hat{\boldsymbol x}_k^-)=H^+\boldsymbol z_k=\red{\hat{\boldsymbol x}_{k_{MEA}}}\)

### 1.6 误差协方差矩阵

我们在计算卡尔曼增益 \(K_k\) 的时候出现了 \(E\left(\boldsymbol e_k^-{\boldsymbol e_k^-}^T\right)\)，并使用 \(P_k^-\) 进行表示，代表先验的误差协方差矩阵，现在要做的就是求出先验误差协方差矩阵 \(P_k^-\)，同样可以由协方差矩阵的定义出发。

首先先求 \(\boldsymbol e_k^-\)

\[\begin{align}\boldsymbol e_k^-&=\boldsymbol x_k-\hat{\boldsymbol x}_k^-\\ &=A\boldsymbol x_{k-1}+B\boldsymbol u_{k-1}+\boldsymbol w_{k-1}-A\hat{\boldsymbol x}_{k-1}-B\boldsymbol u_{k-1}\\ &=A(\boldsymbol x_{k-1}-\hat{\boldsymbol x}_{k-1})+\boldsymbol w_{k-1}\\ &=A\boldsymbol e_{k-1}+\boldsymbol w_{k-1}\tag{1-26}\end{align}\]

因此先验误差协方差矩阵 \(P_k^-\) 可以表示为

\[\begin{align}P_k^- &=E\left(\boldsymbol e_k^-{\boldsymbol e_k^-}^T\right)=E\left[(A\boldsymbol e_{k-1}+\boldsymbol w_{k-1})(A\boldsymbol e_{k-1}+\boldsymbol w_{k-1})^T\right]\\ &=E\left[(A\boldsymbol e_{k-1}+\boldsymbol w_{k-1})(\boldsymbol e_{k-1}^TA^T+\boldsymbol w_{k-1}^T)\right]\\ &=E\left[A\boldsymbol e_{k-1}\boldsymbol e_{k-1}^TA^T+A\boldsymbol e_{k-1}\boldsymbol w_{k-1}^T+\boldsymbol w_{k-1}\boldsymbol e_{k-1}^TA^T+\boldsymbol w_{k-1}\boldsymbol w_{k-1}^T\right]\\ &=AE(\boldsymbol e_{k-1}\boldsymbol e_{k-1}^T)A^T+AE\boldsymbol e_{k-1}E\boldsymbol w_{k-1}^T+E\boldsymbol w_{k-1}E\boldsymbol e_{k-1}^TA^T+E(\boldsymbol w_{k-1}\boldsymbol w_{k-1}^T)\\ &=AP_{k-1}A^T+0+0+Q \tag{1-27}\end{align}\]

最终可以得到先验误差协方差矩阵的表达式

\[\red{P_k^-=AP_{k-1}A^T+Q\tag{1-28}}\]

在求解 \(P_k^-\) 的时候用到了 \(P_{k-1}\)，因此需要进一步求解 \(P_k\)，从而为下一次 \(P_{k+1}^-\) 所使用。

由 1.5 卡尔曼增益推导 的公式 \(\fml{1-20}\) 可以得到

\[\begin{align}P_k &=\green{P_k^-}-\red{K_kHP_k^-}-\green{P_k^-}\orange{H^TK_k^T}+\red{K_kHP_k^-}\orange{H^TK_k^T}+K_kRK_k^T\\ &=P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_k(HP_k^-H^T+R)K_k^T\\ 代入\fml{1-25}\quad&=P_k^--K_kHP_k^--P_k^-H^TK_k^T+P_k^-H^TK_k^T\\ &=P_k^--K_kHP_k^-\tag{1-29}\end{align}\]

即所谓后验误差协方差矩阵 \(P_k\)

\[\red{P_k=(I-K_kH)P_k^-\tag{1-30}}\]

### 1.7 汇总

至此，Kalman Filter 的 5 大公式已经全部求出，分别是公式 \(\fml{1-14}\)、公式 \(\fml{1-17}\)、公式 \(\fml{1-25}\)、公式 \(\fml{1-28}\) 和公式 \(\fml{1-30}\)

按照处理顺序，卡尔曼滤波器划分为两个部分

① 预测

1. 先验状态估计

   \[\hat{\boldsymbol x}_k^-=A\boldsymbol x_{k-1}+B\boldsymbol u_{k-1}\]

2. 计算先验误差协方差

   \[P_k^-=AP_{k-1}A^T+Q\]

② 校正（更新）

1. 计算卡尔曼增益

   \[K_k=P_k^-H^T\left(HP_k^-H^T+R\right)^{-1}\]

2. 后验状态估计

   \[\hat{\boldsymbol x}_k=\hat{\boldsymbol x}_k^-+K_k(\boldsymbol z_k-H\hat{\boldsymbol x}_k^-)\]

3. 更新后验误差协方差

   \[P_k=(I-K_kH)P_k^-\]

## 2 卡尔曼滤波模块的用法

### 2.1 如何配置

首先必须要寻找 RMVL 包，即 find_package(RMVL REQUIRED)，之后可直接在 CMakeLists.txt 中链接库

target_link_libraries(
xxx
PUBLIC rmvl_algorithm
)

这里的 xxx 为需要链接到 algorithm 模块的目标

### 2.2 如何使用

#### 2.2.1 包含头文件

#include <rmvl/algorithm/kalman.hpp>

#### 2.2.2 创建并初始化卡尔曼滤波器对象

取

- 数据类型 Tp = double
- 状态量阶数 StateDim = 4
- 观测量阶数 MeatureDim = 2
- 无系统输入

可以得到以下代码

rm::KalmanFilter<double, 4, 2> filter; // 简写可以写成 rm::KF42d filter
filter.setR(/* ... */);
filter.setQ(/* ... */);
cv::Vec4f init_vec = {/* ... */};
filter.init(init_vec, pred_err);
rm::KalmanFilterStaticDatas::setQ
void setQ(const cv::Matx< Tp, StateDim, StateDim > &process_err)
设置过程噪声协方差矩阵
定义 kalman.hpp:90
rm::KalmanFilterStaticDatas::setR
void setR(const cv::Matx< Tp, MeasureDim, MeasureDim > &measure_err)
设置测量噪声协方差矩阵
定义 kalman.hpp:83
rm::KalmanFilterStaticDatas::init
void init(const cv::Matx< Tp, StateDim, 1 > &x0, Tp error)
初始化状态以及对应的误差协方差矩阵（常数对角矩阵）
定义 kalman.hpp:60
rm::KalmanFilter
卡尔曼滤波器
定义 kalman.hpp:120

#### 2.2.3 运行

filter.setA(/* ... */);
filter.setH(/* ... */);
// Prediction
filter.predict();
// Correction
cv::Vec2f zk = {/* ... */};
auto corr = filter.correct(zk);
rm::KalmanFilter::correct
auto correct(const cv::Matx< Tp, MeasureDim, 1 > &zk)
卡尔曼滤波器校正部分，包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计
定义 kalman.hpp:186
rm::KalmanFilter::predict
auto predict()
卡尔曼滤波的预测部分，包括状态量的先验估计和误差协方差的先验估计
定义 kalman.hpp:169
rm::KalmanFilter::setH
void setH(const cv::Matx< Tp, MeasureDim, StateDim > &observe_tf)
设置观测矩阵
定义 kalman.hpp:161
rm::KalmanFilter::setA
void setA(const cv::Matx< Tp, StateDim, StateDim > &state_tf)
设置状态转移矩阵
定义 kalman.hpp:150

### 参考文献

- 卡尔曼滤波 [9]