卡尔曼滤波器(20) -- 多维卡尔曼滤波器(例9)

This blog is translated from https://www.kalmanfilter.net/default.aspx.
It’s an excellent tutorial about Kalman Filter written by Alex Becker.

目录
卡尔曼滤波器(1) – 背景知识
卡尔曼滤波器(2) – α−β−γ滤波器(例1)
卡尔曼滤波器(3) – α−β−γ滤波器(例2)
卡尔曼滤波器(4) – α−β−γ滤波器(例3&例4&总结)
卡尔曼滤波器(5) – 一维卡尔曼滤波器(介绍)
卡尔曼滤波器(6) – 一维卡尔曼滤波器(例5&完整模型)
卡尔曼滤波器(7) – 一维卡尔曼滤波器(例6)
卡尔曼滤波器(8) – 一维卡尔曼滤波器(例7&例8)
卡尔曼滤波器(9) – 多维卡尔曼滤波器(前言&预备)
卡尔曼滤波器(10) – 多维卡尔曼滤波器(状态外推方程)
卡尔曼滤波器(11) – 多维卡尔曼滤波器(线性动态系统模型)
卡尔曼滤波器(12) – 多维卡尔曼滤波器(协方差外推方程)
卡尔曼滤波器(13) – 多维卡尔曼滤波器(测量方程)
卡尔曼滤波器(14) – 多维卡尔曼滤波器(简短总结)
卡尔曼滤波器(15) – 多维卡尔曼滤波器(状态更新方程)
卡尔曼滤波器(16) – 多维卡尔曼滤波器(协方差更新方程)
卡尔曼滤波器(17) – 多维卡尔曼滤波器(卡尔曼增益)
卡尔曼滤波器(18) – 多维卡尔曼滤波器(简化的协方差更新方程)
卡尔曼滤波器(19) – 多维卡尔曼滤波器(总结)
卡尔曼滤波器(20) – 多维卡尔曼滤波器(例9)

---

这是多维卡尔曼滤波器的最后一部分

有2个例子，第1个是无控制输入的六维卡尔曼滤波器，第2个是有控制输入的二维卡尔曼滤波器。

例9 - 车子位置估计

这个例子里将会使用我们目前学过的知识，讲一下多维卡尔曼滤波器的实现方法。

这里我们来估计车子在 $XY$ 平面的位置。

车子有个板载定位传感器，可以报告系统的 $X$ 和 $Y$ 坐标。

假设系统是恒定加速度的。

状态外推方程

现在我们可以导出状态外推方程。 矩阵表示法中状态外推方程的一般形式为：$${\hat{x}}_{n+1,n}\text{x^n+1,n}{\hat{x}}_{n+1,n}=F\text{F}F{\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}+G{u}_n\text{Gun}G{u}_n+w_n\text{wn}{w}_n$$其中：
${\hat{x}}_{n+1,n}\text{x^n+1,n}{\hat{x}}_{n+1,n}$ 是在时间点 $n+1$ 的预测状态向量
${\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}$ 是在时间点 $n$ 的估计状态向量
$u_n\text{un}{u}_n$ 是控制变量
$w_n\text{wn}{w}_n$ 是过程噪声
$F\text{F}F$ 是状态过渡矩阵
$G\text{G}G$ 是控制矩阵

在这里例子里状态外推方程可以简化为：$${\hat{x}}_{n+1,n}\text{x^n+1,n}{\hat{x}}_{n+1,n}=F\text{F}F{\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}$$

系统状态 $x_n$ 为：$${\hat{x}}_n\text{x^n}{\hat{x}}_n=\left[\begin{array}{c}{\hat{x}}_n\\ {\hat{\dot{x}}}_n\\ {\hat{\ddot{x}}}_n\\ {\hat{y}}_n\\ {\hat{\dot{y}}}_n\\ {\hat{\ddot{y}}}_n\end{array}\right]$$

到时间点 $n+1$ 的车子外推状态可以用以下公式描述：

$$\{\begin{array}{rl}{\hat{x}}_{n+1,n}& ={\hat{x}}_{n,n}+{\hat{\dot{x}}}_{n,n}\Delta t+\frac{1}{2}{\ddot{x}}_{n,n}\Delta t^2\\ {\hat{\dot{x}}}_{n+1,n}& ={\hat{\dot{x}}}_{n,n}+{\hat{\ddot{x}}}_{n,n}\Delta t\\ {\hat{\ddot{x}}}_{n+1,n}& ={\hat{\ddot{x}}}_{n,n}\\ {\hat{y}}_{n+1,n}& ={\hat{y}}_{n,n}+{\hat{\dot{y}}}_{n,n}\Delta t+\frac{1}{2}{\ddot{y}}_{n,n}\Delta t^2\\ {\hat{\dot{y}}}_{n+1,n}& ={\hat{\dot{y}}}_{n,n}+{\hat{\ddot{y}}}_{n,n}\Delta t\\ {\hat{\ddot{y}}}_{n+1,n}& ={\hat{\ddot{y}}}_{n,n}\end{array}$$

矩阵形式：
$$\left[\begin{array}{c}{\hat{x}}_{n+1,n}\\ {\hat{\dot{x}}}_{n+1,n}\\ {\hat{\ddot{x}}}_{n+1,n}\\ {\hat{y}}_{n+1,n}\\ {\hat{\dot{y}}}_{n+1,n}\\ {\hat{\ddot{y}}}_{n+1,n}\end{array}\right]=\left[\begin{array}{cccccc}1& \Delta t& 0.5\Delta t^2& 0& 0& 0\\ 0& 1& \Delta t& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& \Delta t& 0.5\Delta t^2\\ 0& 0& 0& 0& 1& \Delta t\\ 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{n,n}\\ {\hat{\dot{x}}}_{n,n}\\ {\hat{\ddot{x}}}_{n,n}\\ {\hat{y}}_{n,n}\\ {\hat{\dot{y}}}_{n,n}\\ {\hat{\ddot{y}}}_{n,n}\end{array}\right]$$

协方差外推方程

协方差外推方程的一般形式为：$$P_{n+1,n}=F{P}_{n,n}{F}^T+Q\text{Pn+1,n=FPn,nFT+Q}{P}_{n+1,n}=F{P}_{n,n}{F}^T+Q$$其中：
$P_{n,n}\text{Pn,n}{P}_{n,n}$ 是当前状态的估计不确定性（协方差）矩阵
$P_{n+1,n}\text{Pn+1,n}{P}_{n+1,n}$ 是在当前预测的下一个状态的估计不确定性（协方差）矩阵
$F\text{F}F$ 是之前在“线性动态系统模型”部分推导出来的状态转移矩阵
$Q\text{Q}Q$ 是过程噪声矩阵

估计不确定性的矩阵形式为：$$P\text{P}P=\left[\begin{array}{cccccc}{p}_x& p_{x\dot{x}}& p_{x\ddot{x}}& p_{xy}& p_{x\dot{y}}& p_{x\ddot{y}}\\ p_{\dot{x}x}& p_{\dot{x}}& p_{\dot{x}\ddot{x}}& p_{\dot{x}y}& p_{\dot{x}\dot{y}}& p_{\dot{x}\ddot{y}}\\ p_{\ddot{x}x}& p_{\ddot{x}\dot{x}}& p_{\ddot{x}}& p_{\ddot{x}y}& p_{\ddot{x}\dot{y}}& p_{\ddot{x}\ddot{y}}\\ p_{yx}& p_{y\dot{x}}& p_{y\ddot{x}}& p_y& p_{y\dot{y}}& p_{y\ddot{y}}\\ p_{\dot{y}x}& p_{\dot{y}\dot{x}}& p_{\dot{y}\ddot{x}}& p_{\dot{y}y}& p_{\dot{y}}& p_{\dot{y}\ddot{y}}\\ p_{\ddot{y}x}& p_{\ddot{y}\dot{x}}& p_{\ddot{y}\ddot{x}}& p_{\ddot{y}y}& p_{\ddot{y}\dot{y}}& p_{\ddot{y}}\end{array}\right]$$

矩阵的对角线是估计的方差：
$\bullet$  $p_x$ 是 $X$ 轴位置估计的方差
$\bullet$  $p_{\dot{x}}$ 是 $X$ 轴速度估计的方差
$\bullet$  $p_{\ddot{x}}$ 是 $X$ 轴加速度估计的方差
$\bullet$  $p_y$ 是 $Y$ 轴位置估计的方差
$\bullet$  $p_{\dot{y}}$ 是 $Y$ 轴速度估计的方差
$\bullet$  $p_{\ddot{y}}$ 是 $Y$ 轴加速度估计的方差
$\bullet$  非对角线上的是协方差

假设 $X$ 和 $Y$ 轴上的估计误差(estimation errors)不相关，因此可以将相互项设置为零.$$P\text{P}P=\left[\begin{array}{cccccc}{p}_x& p_{x\dot{x}}& p_{x\ddot{x}}& 0& 0& 0\\ p_{\dot{x}x}& p_{\dot{x}}& p_{\dot{x}\ddot{x}}& 0& 0& 0\\ p_{\ddot{x}x}& p_{\ddot{x}\dot{x}}& p_{\ddot{x}}& 0& 0& 0\\ 0& 0& 0& p_y& p_{y\dot{y}}& p_{y\ddot{y}}\\ 0& 0& 0& p_{\dot{y}y}& p_{\dot{y}}& p_{\dot{y}\ddot{y}}\\ 0& 0& 0& p_{\ddot{y}y}& p_{\ddot{y}\dot{y}}& p_{\ddot{y}}\end{array}\right]$$

过程噪声矩阵

假设是离散噪声模型：不同时间片的噪声是不同的，但是在单个时间片内是不变的。
二维恒定加速度模型的过程噪声矩阵如下所示：$$Q\text{Q}Q=\left[\begin{array}{cccccc}{\sigma }_x^2& {\sigma }_{x\dot{x}}^2& {\sigma }_{x\ddot{x}}^2& {\sigma }_{xy}^2& {\sigma }_{x\dot{y}}^2& {\sigma }_{x\ddot{y}}^2\\ {\sigma }_{\dot{x}x}^2& {\sigma }_{\dot{x}}^2& {\sigma }_{\dot{x}\ddot{x}}^2& {\sigma }_{\dot{x}y}^2& {\sigma }_{\dot{x}\dot{y}}^2& {\sigma }_{\dot{x}\ddot{y}}^2\\ {\sigma }_{\ddot{x}x}^2& {\sigma }_{\ddot{x}\dot{x}}^2& {\sigma }_{\ddot{x}}^2& {\sigma }_{\ddot{x}y}^2& {\sigma }_{\ddot{x}\dot{y}}^2& {\sigma }_{\ddot{x}\ddot{y}}^2\\ {\sigma }_{yx}^2& {\sigma }_{y\dot{x}}^2& {\sigma }_{y\ddot{x}}^2& {\sigma }_y^2& {\sigma }_{y\dot{y}}^2& {\sigma }_{y\ddot{y}}^2\\ {\sigma }_{\dot{y}x}^2& {\sigma }_{\dot{y}\dot{x}}^2& {\sigma }_{\dot{y}\ddot{x}}^2& {\sigma }_{\dot{y}y}^2& {\sigma }_{\dot{y}}^2& {\sigma }_{\dot{y}\ddot{y}}^2\\ {\sigma }_{\ddot{y}x}^2& {\sigma }_{\ddot{y}\dot{x}}^2& {\sigma }_{\ddot{y}\ddot{x}}^2& {\sigma }_{\ddot{y}y}^2& {\sigma }_{\ddot{y}\dot{y}}^2& {\sigma }_{\ddot{y}}^2\end{array}\right]$$

假设 $X$ 轴和 $Y$ 轴的过程噪声是不相关的，则相互项设为 $0$：$$Q\text{Q}Q=\left[\begin{array}{cccccc}{\sigma }_x^2& {\sigma }_{x\dot{x}}^2& {\sigma }_{x\ddot{x}}^2& 0& 0& 0\\ {\sigma }_{\dot{x}x}^2& {\sigma }_{\dot{x}}^2& {\sigma }_{\dot{x}\ddot{x}}^2& 0& 0& 0\\ {\sigma }_{\ddot{x}x}^2& {\sigma }_{\ddot{x}\dot{x}}^2& {\sigma }_{\ddot{x}}^2& 0& 0& 0\\ 0& 0& 0& {\sigma }_y^2& {\sigma }_{y\dot{y}}^2& {\sigma }_{y\ddot{y}}^2\\ 0& 0& 0& {\sigma }_{\dot{y}y}^2& {\sigma }_{\dot{y}}^2& {\sigma }_{\dot{y}\ddot{y}}^2\\ 0& 0& 0& {\sigma }_{\ddot{y}y}^2& {\sigma }_{\ddot{y}\dot{y}}^2& {\sigma }_{\ddot{y}}^2\end{array}\right]$$

前面已经推导了恒定加速度模型的 Q 矩阵，对于我们这个例子，Q 矩阵为：$$Q\text{Q}Q=\left[\begin{array}{cccccc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}& 0& 0& 0\\ \frac{\Delta t^3}{2}& \Delta t^2& \Delta t& 0& 0& 0\\ \frac{\Delta t^2}{2}& \Delta t& 1& 0& 0& 0\\ 0& 0& 0& \frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}\\ 0& 0& 0& \frac{\Delta t^3}{2}& \Delta t^2& \Delta t\\ 0& 0& 0& \frac{\Delta t^2}{2}& \Delta t& 1\end{array}\right]{\sigma }_a^2$$其中：
$\bullet$ $\Delta t$ 是连续测量之间的时间间隔
$\bullet$ ${\sigma }_a^2$ 是加速度中的随机方差

下面给出本例子的协方差外推方程：$$P_{n+1,n}=F{P}_{n,n}{F}^T+Q\text{Pn+1,n=FPn,nFT+Q}{P}_{n+1,n}=F{P}_{n,n}{F}^T+Q$$
$$\left[\begin{array}{cccccc}{p}_{x_{n+1,n}}& p_{x{\dot{x}}_{n+1,n}}& p_{x{\ddot{x}}_{n+1,n}}& 0& 0& 0\\ p_{\dot{x}{x}_{n+1,n}}& p_{{\dot{x}}_{n+1,n}}& p_{\dot{x}{\ddot{x}}_{n+1,n}}& 0& 0& 0\\ p_{\ddot{x}{x}_{n+1,n}}& p_{\ddot{x}{\dot{x}}_{n+1,n}}& p_{{\ddot{x}}_{n+1,n}}& 0& 0& 0\\ 0& 0& 0& p_{y_{n+1,n}}& p_{y{\dot{y}}_{n+1,n}}& p_{y{\ddot{y}}_{n+1,n}}\\ 0& 0& 0& p_{\dot{y}{y}_{n+1,n}}& p_{{\dot{y}}_{n+1,n}}& p_{\dot{y}{\ddot{y}}_{n+1,n}}\\ 0& 0& 0& p_{\ddot{y}{y}_{n+1,n}}& p_{\ddot{y}{\dot{y}}_{n+1,n}}& p_{{\ddot{y}}_{n+1,n}}\end{array}\right]=\left[\begin{array}{cccccc}1& \Delta t& 0.5\Delta t^2& 0& 0& 0\\ 0& 1& \Delta t& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& \Delta t& 0.5\Delta t^2\\ 0& 0& 0& 0& 1& \Delta t\\ 0& 0& 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccccc}{p}_{x_{n,n}}& p_{x{\dot{x}}_{n,n}}& p_{x{\ddot{x}}_{n,n}}& 0& 0& 0\\ p_{\dot{x}{x}_{n,n}}& p_{{\dot{x}}_{n,n}}& p_{\dot{x}{\ddot{x}}_{n,n}}& 0& 0& 0\\ p_{\ddot{x}{x}_{n,n}}& p_{\ddot{x}{\dot{x}}_{n,n}}& p_{{\ddot{x}}_{n,n}}& 0& 0& 0\\ 0& 0& 0& p_{y_{n,n}}& p_{y{\dot{y}}_{n,n}}& p_{y{\ddot{y}}_{n,n}}\\ 0& 0& 0& p_{\dot{y}{y}_{n,n}}& p_{{\dot{y}}_{n,n}}& p_{\dot{y}{\ddot{y}}_{n,n}}\\ 0& 0& 0& p_{\ddot{y}{y}_{n,n}}& p_{\ddot{y}{\dot{y}}_{n,n}}& p_{{\ddot{y}}_{n,n}}\end{array}\right]\times \left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ \Delta t& 1& 0& 0& 0& 0\\ 0.5\Delta t^2& \Delta t& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& \Delta t& 1& 0\\ 0& 0& 0& 0.5\Delta t^2& \Delta t& 1\end{array}\right]+\left[\begin{array}{cccccc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}& 0& 0& 0\\ \frac{\Delta t^3}{2}& \Delta t^2& \Delta t& 0& 0& 0\\ \frac{\Delta t^2}{2}& \Delta t& 1& 0& 0& 0\\ 0& 0& 0& \frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}\\ 0& 0& 0& \frac{\Delta t^3}{2}& \Delta t^2& \Delta t\\ 0& 0& 0& \frac{\Delta t^2}{2}& \Delta t& 1\end{array}\right]{\sigma }_a^2$$

测量方程

矩阵形式的测量方程的一般形式为：$$z_n\text{zn}{z}_n=H\text{H}H{x}_n\text{xn}{x}_n+v_n\text{vn}{v}_n$$其中：
$z_n\text{zn}{z}_n$ 是测量向量（measurement vector）
$x_n\text{xn}{x}_n$ 是真实系统状态（隐藏状态）
$v_n\text{vn}{v}_n$ 是随机噪声向量
$H\text{H}H$ 是观察矩阵(observation matrix)

这里只提供了车子 $X$ 轴和 $Y$ 轴的测量值，因此 $z_n\text{zn}{z}_n=\left[\begin{array}{c}{x}_{n,measured}\\ y_{n,measured}\end{array}\right]$

$$\begin{array}{rl}{z}_n\text{zn}{z}_n& =H\text{H}H{x}_n\text{xn}{x}_n\\ \left[\begin{array}{c}{x}_{n,measured}\\ y_{n,measured}\end{array}\right]& =H\text{H}H\left[\begin{array}{c}{x}_n\\ {\dot{x}}_n\\ {\ddot{x}}_n\\ y_n\\ {\dot{y}}_n\\ {\ddot{y}}_n\end{array}\right]\end{array}$$

$z_n\text{zn}{z}_n$ 的维度是 $2\times 1$， $x_n\text{xn}{x}_n$ 的维度是 $6\times 1$，因此观察矩阵 $H\text{H}H$ 的维度是 $2\times 6$ $$H\text{H}H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right]$$

测量不确定性

测量协方差矩阵为：$$R_n\text{Rn}{R}_n=\left[\begin{array}{cc}{\sigma }_{x_m}^2& {\sigma }_{y{x}_m}^2\\ {\sigma }_{x{y}_m}^2& {\sigma }_{y_m}^2\end{array}\right]$$

下标 $m$ 是测量不确定性

假设测量值 $x$ 和 $y$ 是不相关的，即 $x$ 坐标测量值的误差不取决于 $y$ 坐标测量值的误差。$$R_n\text{Rn}{R}_n=\left[\begin{array}{cc}{\sigma }_{x_m}^2& 0\\ 0& {\sigma }_{y_m}^2\end{array}\right]$$

在实际应用中，测量不确定度可能因测量不同而不同。在许多系统中，测量不确定度取决于测量信噪比、传感器与目标的夹角、信号频率等参数。

为了让例子更简单，我们将假设一个恒定的测量不确定性：$$R_1\text{R1}{R}_1=R_2\text{R2}{R}_2\dots R_{n-1}\text{Rn−1}{R}_{n-1}=R_n\text{Rn}{R}_n=R\text{R}R$$

卡尔曼增益

矩阵形式的卡尔曼增益如下：$$K_n\text{Kn}{K}_n=P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}{H\text{H}H}^T({H{P}_{n,n-1}H\text{HPn,n−1H}H{P}_{n,n-1}H}^T+R_n\text{Rn}{R}_n{)}^{-1}$$其中：
$K_n\text{Kn}{K}_n$ 是卡尔曼增益
$P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}$ 是在当前时刻的先前估计不确定性（方差）矩阵（在上一个状态预测得到的）
$H\text{H}H$ 是观察矩阵
$R_n\text{Rn}{R}_n$ 是测量不确定性（测量噪声协方差矩阵）

$$\left[\begin{array}{cc}{K}_{1,1_n}& K_{1,2_n}\\ K_{2,1_n}& K_{2,2_n}\\ K_{3,1_n}& K_{3,2_n}\\ K_{4,1_n}& K_{4,2_n}\\ K_{5,1_n}& K_{5,2_n}\\ K_{6,1_n}& K_{6,2_n}\end{array}\right]=\left[\begin{array}{cccccc}{p}_{x_{n,n-1}}& p_{x{\dot{x}}_{n,n-1}}& p_{x{\ddot{x}}_{n,n-1}}& 0& 0& 0\\ p_{\dot{x}{x}_{n,n-1}}& p_{{\dot{x}}_{n,n-1}}& p_{\dot{x}{\ddot{x}}_{n,n-1}}& 0& 0& 0\\ p_{\ddot{x}{x}_{n,n-1}}& p_{\ddot{x}{\dot{x}}_{n,n-1}}& p_{{\ddot{x}}_{n,n-1}}& 0& 0& 0\\ 0& 0& 0& p_{y_{n,n-1}}& p_{y{\dot{y}}_{n,n-1}}& p_{y{\ddot{y}}_{n,n-1}}\\ 0& 0& 0& p_{\dot{y}{y}_{n,n-1}}& p_{{\dot{y}}_{n,n-1}}& p_{\dot{y}{\ddot{y}}_{n,n-1}}\\ 0& 0& 0& p_{\ddot{y}{y}_{n,n-1}}& p_{\ddot{y}{\dot{y}}_{n,n-1}}& p_{{\ddot{y}}_{n,n-1}}\end{array}\right]\left[\begin{array}{cc}1& 0\\ 0& 0\\ 0& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right]\times {\left(\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right]\left[\begin{array}{cccccc}{p}_{x_{n,n-1}}& p_{x{\dot{x}}_{n,n-1}}& p_{x{\ddot{x}}_{n,n-1}}& 0& 0& 0\\ p_{\dot{x}{x}_{n,n-1}}& p_{{\dot{x}}_{n,n-1}}& p_{\dot{x}{\ddot{x}}_{n,n-1}}& 0& 0& 0\\ p_{\ddot{x}{x}_{n,n-1}}& p_{\ddot{x}{\dot{x}}_{n,n-1}}& p_{{\ddot{x}}_{n,n-1}}& 0& 0& 0\\ 0& 0& 0& p_{y_{n,n-1}}& p_{y{\dot{y}}_{n,n-1}}& p_{y{\ddot{y}}_{n,n-1}}\\ 0& 0& 0& p_{\dot{y}{y}_{n,n-1}}& p_{{\dot{y}}_{n,n-1}}& p_{\dot{y}{\ddot{y}}_{n,n-1}}\\ 0& 0& 0& p_{\ddot{y}{y}_{n,n-1}}& p_{\ddot{y}{\dot{y}}_{n,n-1}}& p_{{\ddot{y}}_{n,n-1}}\end{array}\right]\left[\begin{array}{cc}1& 0\\ 0& 0\\ 0& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right]+\left[\begin{array}{cc}{\sigma }_{x_m}^2& 0\\ 0& {\sigma }_{y_m}^2\end{array}\right]\right)}^{-1}$$

状态更新方程

状态更新方程的矩阵形式为：$${\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}={\hat{x}}_{n,n-1}\text{x^n,n−1}{\hat{x}}_{n,n-1}+K_n\text{Kn}{K}_n(z_n\text{zn}{z}_n-H\text{H}H{\hat{x}}_{n,n-1}\text{x^n,n−1}{\hat{x}}_{n,n-1})$$其中：
${\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}$ 是在时刻 $n$ 的估计系统状态向量
${\hat{x}}_{n,n-1}\text{x^n,n−1}{\hat{x}}_{n,n-1}$ 是在时刻 $n-1$ 的预测系统状态向量
$K_n\text{Kn}{K}_n$ 是卡尔曼增益
$z_n\text{zn}{z}_n$ 是测量值
$H\text{H}H$ 是观察矩阵(observation matrix)

构成状态更新方程的元素已经在前面全部定义好了。

协方差更新方程

$$P_{n,n}\text{Pn,n}{P}_{n,n}=(I\text{I}I-K_{n}H\text{KnH}{K}_{n}H)P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}(I\text{I}I-K_{n}H\text{KnH}{K}_{n}H{)}^T+K_n\text{Kn}{K}_n{R}_n\text{Rn}{R}_n{K_n\text{Kn}{K}_n}^T$$ 其中
$P_{n,n}\text{Pn,n}{P}_{n,n}$ 是当前状态的估计不确定性(协方差)矩阵
$P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}$ 是在当前状态下，先前估计不确定性(协方差)矩阵 （在上一个状态时预测得到的）
$K_n\text{Kn}{K}_n$ 是卡尔曼增益
$H\text{H}H$ 是观察矩阵
$R_n\text{Rn}{R}_n$ 是测量不确定性（测量噪声协方差矩阵）

构成协方差更新方程的所有元素已经在前面全部定义好了。

数值实例

假设车子以恒定的速度沿着 $X$ 轴的方向直线行驶。跑了 $300$ 米后向右转，转弯半径为 $300$ 米。转弯的时候由于圆周运动而受到加速度（角加速度）。

车子的运动如图所示：

$\bullet$ 测量时间间隔： $\Delta t=1s$
$\bullet$ 随机加速度标准差：${\sigma }_a^2=0.15\frac{m}{s^2}$
$\bullet$ 测量误差标准差：${\sigma }_{x_m}={\sigma }_{y_m}=3m$
$\bullet$ 状态过渡矩阵 $F$ ：$$F\text{F}F=\left[\begin{array}{cccccc}1& \Delta t& 0.5\Delta t^2& 0& 0& 0\\ 0& 1& \Delta t& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& \Delta t& 0.5\Delta t^2\\ 0& 0& 0& 0& 1& \Delta t\\ 0& 0& 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccccc}1& 1& 0.5& 0& 0& 0\\ 0& 1& 1& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 1& 0.5\\ 0& 0& 0& 0& 1& 1\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$
$\bullet$ 过程噪声矩阵 $Q\text{Q}Q$：$$Q\text{Q}Q=\left[\begin{array}{cccccc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}& 0& 0& 0\\ \frac{\Delta t^3}{2}& \Delta t^2& \Delta t& 0& 0& 0\\ \frac{\Delta t^2}{2}& \Delta t& 1& 0& 0& 0\\ 0& 0& 0& \frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}\\ 0& 0& 0& \frac{\Delta t^3}{2}& \Delta t^2& \Delta t\\ 0& 0& 0& \frac{\Delta t^2}{2}& \Delta t& 1\end{array}\right]{\sigma }_a^2=\left[\begin{array}{cccccc}\frac{1}{4}& \frac{1}{2}& \frac{1}{2}& 0& 0& 0\\ \frac{1}{2}& 1& 1& 0& 0& 0\\ \frac{1}{2}& 1& 1& 0& 0& 0\\ 0& 0& 0& \frac{1}{4}& \frac{1}{2}& \frac{1}{2}\\ 0& 0& 0& \frac{1}{2}& 1& 1\\ 0& 0& 0& \frac{1}{2}& 1& 1\end{array}\right]0.15^2$$

$\bullet$ 测量不确定性矩阵 $R\text{R}R$ 为： $$R_n\text{Rn}{R}_n=\left[\begin{array}{cc}{\sigma }_{x_m}^2& 0\\ 0& {\sigma }_{y_m}^2\end{array}\right]=\left[\begin{array}{cc}9& 0\\ 0& 9\end{array}\right]$$

下面的表格记录了35次有噪声的测量：

	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
x(m)	-393.66	-375.93	-351.04	-328.96	-299.35	-273.36	-245.89	-222.58	-198.03	-174.17	-146.32	-123.72	-103.47	-78.23	-52.65	-23.34	25.96	49.72	76.94	95.38	119.83	144.01	161.84	180.56	201.42	222.62	239.4	252.51	266.26	271.75	277.4	294.12	301.23	291.8	299.89
y(m)	300.4	301.78	295.1	305.19	301.06	302.05	300	303.57	296.33	297.65	297.41	299.61	299.6	302.39	295.04	300.09	294.72	298.61	294.64	284.88	272.82	264.93	251.46	241.27	222.98	203.73	184.1	166.12	138.71	119.71	100.41	79.76	50.62	32.99	2.14

第 0 次迭代

初始化

我们不知道车子的位置；初始位置，车速，加速度都设为 $0$：

$${\hat{x}}_{0,0}\text{x^0,0}{\hat{x}}_{0,0}=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]$$

由于初始状态向量是个猜测值，我们将会设置很高的估计不确定性。高的估计不确定性会导致高卡尔曼增益，给到测量值很高的权重。$$P_{0,0}\text{P0,0}{P}_{0,0}=\left[\begin{array}{cccccc}500& 0& 0& 0& 0& 0\\ 0& 500& 0& 0& 0& 0\\ 0& 0& 500& 0& 0& 0\\ 0& 0& 0& 500& 0& 0\\ 0& 0& 0& 0& 500& 0\\ 0& 0& 0& 0& 0& 500\end{array}\right]$$

预测

现在我们可以基于初始值预测下一个状态：$${\hat{x}}_{1,0}\text{x^1,0}{\hat{x}}_{1,0}=F\text{F}F{\hat{x}}_{0,0}\text{x^0,0}{\hat{x}}_{0,0}=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]$$

$$P_{1,0}\text{P1,0}{P}_{1,0}=F\text{F}F{P}_{0,0}\text{P0,0}{P}_{0,0}{F\text{F}F}^T+Q\text{Q}Q=\left[\begin{array}{cccccc}1125& 750& 250& 0& 0& 0\\ 750& 1000& 500& 0& 0& 0\\ 250& 500& 500& 0& 0& 0\\ 0& 0& 0& 1125& 750& 250\\ 0& 0& 0& 750& 1000& 500\\ 0& 0& 0& 250& 500& 500\end{array}\right]$$

第 1 次迭代

步骤1 - 测量

测量值：$$z_1\text{z1}{z}_1=\left[\begin{array}{c}-375.93\\ 301.78\end{array}\right]$$

步骤2 - 更新

卡尔曼增益的计算：$$K_1\text{K1}{K}_1=P_{1,0}\text{P1,0}{P}_{1,0}{H\text{H}H}^T(H\text{H}H{P}_{1,0}\text{P1,0}{P}_{1,0}{H\text{H}H}^T+R\text{R}R{)}^{-1}=\left[\begin{array}{cc}0.9921& 0\\ 0.6614& 0\\ 0.2205& 0\\ 0& 0.9921\\ 0& 0.6614\\ 0& 0.2205\end{array}\right]$$

对于位置的卡尔曼增益是 $0.9921$，意味着这一次测量的权重远大于估计的权重，估计的权重几乎可以忽略。

估计当前状态：$${\hat{x}}_{1,1}\text{x^1,1}{\hat{x}}_{1,1}={\hat{x}}_{1,0}\text{x^1,0}{\hat{x}}_{1,0}+K_1\text{K1}{K}_1(z_1\text{z1}{z}_1-H\text{H}H{\hat{x}}_{1,0}\text{x^1,0}{\hat{x}}_{1,0})=\left[\begin{array}{c}-390.54\\ -260.36\\ -86.8\\ 298.02\\ 198.7\\ 66.23\end{array}\right]$$

更新当前估计不确定性：$$P_{1,1}\text{P1,1}{P}_{1,1}=(I\text{I}I-K_{1}H\text{K1H}{K}_{1}H)P_{1,0}\text{P1,0}{P}_{1,0}(I\text{I}I-K_{1}H\text{K1H}{K}_{1}H{)}^T+K_1\text{K1}{K}_{1}R\text{R}R{K_1\text{K1}{K}_1}^T=\left[\begin{array}{cccccc}8.93& 750& 2& 0& 0& 0\\ 750& 504& 334.7& 0& 0& 0\\ 2& 334.7& 444.9& 0& 0& 0\\ 0& 0& 0& 8.93& 750& 2\\ 0& 0& 0& 750& 504& 334.7\\ 0& 0& 0& 2& 334.7& 444.9\end{array}\right]$$

步骤3 - 预测

$${\hat{x}}_{2,1}\text{x^2,1}{\hat{x}}_{2,1}=F\text{F}F{\hat{x}}_{1,1}\text{x^1,1}{\hat{x}}_{1,1}=\left[\begin{array}{c}-694.3\\ -347.15\\ -86.8\\ 529.8\\ 264.9\\ 66.23\end{array}\right]$$

$$P_{2,1}\text{P2,1}{P}_{2,1}=F\text{F}F{P}_{1,1}\text{P1,1}{P}_{1,1}{F\text{F}F}^T+Q\text{Q}Q=\left[\begin{array}{cccccc}972& 1236& 559& 0& 0& 0\\ 1236& 1618& 780& 0& 0& 0\\ 559& 780& 445& 0& 0& 0\\ 0& 0& 0& 972& 1236& 559\\ 0& 0& 0& 1236& 1618& 780\\ 0& 0& 0& 559& 780& 445\end{array}\right]$$

预测不确定性仍然非常高。

第 2 次迭代

步骤1 - 测量

测量值：$$z_2\text{z2}{z}_2=\left[\begin{array}{c}-393.66\\ 300.4\end{array}\right]$$

步骤2 - 更新

卡尔曼增益的计算：$$K_2\text{K2}{K}_2=P_{2,1}\text{P2,1}{P}_{2,1}{H\text{H}H}^T(H\text{H}H{P}_{2,1}\text{P2,1}{P}_{2,1}{H\text{H}H}^T+R\text{R}R{)}^{-1}=\left[\begin{array}{cc}0.9908& 0\\ 1.26& 0\\ 0.57& 0\\ 0& 0.9908\\ 0& 1.26\\ 0& 0.57\end{array}\right]$$

估计当前状态：$${\hat{x}}_{2,2}\text{x^2,2}{\hat{x}}_{2,2}={\hat{x}}_{2,1}\text{x^2,1}{\hat{x}}_{2,1}+K_2\text{K2}{K}_2(z_2\text{z2}{z}_2-H\text{H}H{\hat{x}}_{2,1}\text{x^2,1}{\hat{x}}_{2,1})=\left[\begin{array}{c}-378.9\\ 53.8\\ 94.5\\ 303.9\\ -22.3\\ -63.6\end{array}\right]$$

更新当前估计不确定性：$$P_{2,2}\text{P2,2}{P}_{2,2}=(I\text{I}I-K_{2}H\text{K2H}{K}_{2}H)P_{2,1}\text{P2,1}{P}_{2,1}(I\text{I}I-K_{2}H\text{K2H}{K}_{2}H{)}^T+K_2\text{K2}{K}_{2}R\text{R}R{K_2\text{K2}{K}_2}^T=\left[\begin{array}{cccccc}8.92& 11.33& 5.13& 0& 0& 0\\ 11.33& 61.1& 75.4& 0& 0& 0\\ 5.13& 75.4& 126.5& 0& 0& 0\\ 0& 0& 0& 8.92& 11.33& 5.13\\ 0& 0& 0& 11.33& 61.1& 75.4\\ 0& 0& 0& 5.13& 75.4& 126.5\end{array}\right]$$

步骤3 - 预测

$${\hat{x}}_{3,2}\text{x^3,2}{\hat{x}}_{3,2}=F\text{F}F{\hat{x}}_{2,2}\text{x^2,2}{\hat{x}}_{2,2}=\left[\begin{array}{c}-277.8\\ 148.3\\ 94.5\\ 249.8\\ -85.9\\ -63.6\end{array}\right]$$

$$P_{3,2}\text{P3,2}{P}_{3,2}=F\text{F}F{P}_{2,2}\text{P2,2}{P}_{2,2}{F\text{F}F}^T+Q\text{Q}Q=\left[\begin{array}{cccccc}204.9& 254& 143.8& 0& 0& 0\\ 254& 338.5& 202& 0& 0& 0\\ 143.8& 202& 126.5& 0& 0& 0\\ 0& 0& 0& 204.9& 254& 143.8\\ 0& 0& 0& 254& 338.5& 202\\ 0& 0& 0& 143.8& 202& 126.5\end{array}\right]$$

此时预测不确定性依然很高，是时候跳到最后一轮迭代看看了。

第 35 次迭代

步骤1 - 测量

测量值：$$z_{35}\text{z35}{z}_{35}=\left[\begin{array}{c}299.89\\ 2.14\end{array}\right]$$

步骤2 - 更新

卡尔曼增益的计算：$$K_{35}\text{K35}{K}_{35}=P_{35,34}\text{P35,34}{P}_{35,34}{H\text{H}H}^T(H\text{H}H{P}_{35,34}\text{P35,34}{P}_{35,34}{H\text{H}H}^T+R\text{R}R{)}^{-1}=\left[\begin{array}{cc}0.5556& 0\\ 0.2222& 0\\ 0.0444& 0\\ 0& 0.5556\\ 0& 0.2222\\ 0& 0.0444\end{array}\right]$$

对于位置的卡尔曼增益已经收敛于 $0.56$，这意味着测量权重和估计权重几乎相同。

估计当前状态：$${\hat{x}}_{35,35}\text{x^35,35}{\hat{x}}_{35,35}={\hat{x}}_{35,34}\text{x^35,34}{\hat{x}}_{35,34}+K_{35}\text{K35}{K}_{35}(z_{35}\text{z35}{z}_{35}-H\text{H}H{\hat{x}}_{35,34}\text{x^35,34}{\hat{x}}_{35,34})=\left[\begin{array}{c}299.2\\ 0.25\\ -1.9\\ 3.3\\ -25.5\\ -0.64\end{array}\right]$$

更新当前估计不确定性：$$P_{35,35}\text{P35,35}{P}_{35,35}=(I\text{I}I-K_{35}H\text{K35H}{K}_{35}H)P_{35,34}\text{P35,34}{P}_{35,34}(I\text{I}I-K_{35}H\text{K35H}{K}_{35}H{)}^T+K_{35}\text{K35}{K}_{35}R\text{R}R{K_{35}\text{K35}{K}_{35}}^T=\left[\begin{array}{cccccc}5& 2& 0.4& 0& 0& 0\\ 2& 1.4& 0.4& 0& 0& 0\\ 0.4& 0.4& 0.16& 0& 0& 0\\ 0& 0& 0& 5& 2& 0.4\\ 0& 0& 0& 2& 1.4& 0.4\\ 0& 0& 0& 0.4& 0.4& 0.16\end{array}\right]$$

此刻位置不确定性 $p_x=p_y=5$，意味着预测的标准差为 $\sqrt{5}m$ 。

步骤3 - 预测

$${\hat{x}}_{36,35}\text{x^36,35}{\hat{x}}_{36,35}=F\text{F}F{\hat{x}}_{35,35}\text{x^35,35}{\hat{x}}_{35,35}=\left[\begin{array}{c}298.5\\ -1.65\\ -1.9\\ -22.5\\ -26.1\\ -0.64\end{array}\right]$$

$$P_{36,35}\text{P36,35}{P}_{36,35}=F\text{F}F{P}_{35,35}\text{P35,35}{P}_{35,35}{F\text{F}F}^T+Q\text{Q}Q=\left[\begin{array}{cccccc}11.25& 4.5& 0.9& 0& 0& 0\\ 4.5& 2.4& 0.6& 0& 0& 0\\ 0.9& 0.6& 0.2& 0& 0& 0\\ 0& 0& 0& 11.25& 4.5& 0.9\\ 0& 0& 0& 4.5& 2.4& 0.6\\ 0& 0& 0& 0.9& 0.6& 0.2\end{array}\right]$$

本例总结

下表比较了车辆位置的真实值，测量值，估计值。

可以看到，卡尔曼滤波器很好地跟踪了车子，但是，当车子开始转弯操作时，估算值并不是那么准确。一段时间后，卡尔曼滤波器的精度提高了。

车辆沿直线行驶时，加速度恒定且等于零。但是，在转弯操作期间，车辆由于圆周运动而受到加速度：角加速度。

回想一下物理知识，角加速度为：$\alpha =\frac{\Delta V}{R\Delta t}$，其中：$\Delta t$ 是时间间隔，$\Delta V$ 是一角间隔内的速度差，$R$ 是圆半径。

尽管角加速度是恒定的，但是 $x$ 和 $y$ 轴上的角加速度投影不是恒定的，因此 $\ddot{x}$ 和 $\ddot{y}$ 并不是恒定的。

我们这次的卡尔曼滤波器专为恒定加速度模型而设计，尽管这里出现了角加速度，由于正确选择了 ${\sigma }_a^2$ 参数，它仍能成功跟踪机动的车子。

读者可以试试用程序实现这个例子，看看不同的 $R$ 的 ${\sigma }_a^2$ 如何影响实际的卡尔曼滤波器的精度、卡尔曼增益收敛、估计不确定性。

---

例10 - 车子位置估计

这个例子里我们将估计火箭的高度，火箭上面有一个仪器，可以提供火箭高度的测量值。另外，还有一个加速度计，可以测量火箭的加速度。

加速计用作卡尔曼滤波器的控制输入（control input）.

假设模型是恒定加速度的。

加速度计不能感应重力，所以我们需要每次从加速度计的测量值中，减去重力加速度 $g$ 。

例如，把加速度计静止放在桌子上时，它会测到 $1$ 个 $g$ 的上升力，自由落体的加速度计测到的结果是 $0$。

在时间点 $1$ 时，加速度计的测量值为：$$a_n=\ddot{x}-g+ϵ$$其中：

$\bullet$ $\ddot{x}$ 是目标的实际加速度（目标的位置的二阶导）
$\bullet$ $g$ 是重力加速度常量；$g=-9.8\frac{m}{s^2}$
$\bullet$ $ϵ$ 是加速度计的测量误差

状态外推方程

矩阵表示法中状态外推方程的一般形式为：$${\hat{x}}_{n+1,n}\text{x^n+1,n}{\hat{x}}_{n+1,n}=F\text{F}F{\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}+G{u}_n\text{Gun}G{u}_n+w_n\text{wn}{w}_n$$其中：
${\hat{x}}_{n+1,n}\text{x^n+1,n}{\hat{x}}_{n+1,n}$ 是在时间点 $n+1$ 的预测状态向量
${\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}$ 是在时间点 $n$ 的估计状态向量
$u_n\text{un}{u}_n$ 是控制变量
$w_n\text{wn}{w}_n$ 是过程噪声
$F\text{F}F$ 是状态过渡矩阵
$G\text{G}G$ 是控制矩阵

在本例中，我们有控制变量 $u\text{u}u$，它基于加速度计的测量值。

系统的状态 $x_n\text{xn}{x}_n$ 定义为：$$x_n\text{xn}{x}_n=\left[\begin{array}{c}{x}_n\\ {\dot{x}}_n\end{array}\right]$$ 其中：
$x_n$ 是在时间点 $n$ 时火箭的高度
${\dot{x}}_n$ 是在时间点 $n$ 时火箭的速度

我们可以将状态外推方程表示为：$$\left[\begin{array}{c}{\hat{x}}_{n+1,n}\\ {\hat{\dot{x}}}_{n+1,n}\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{x}}_{n,n}\\ {\hat{\dot{x}}}_{n,n}\end{array}\right]+\left[\begin{array}{c}0.5\Delta t^2\\ \Delta t\end{array}\right](a_n+g)$$

上述方程中：$$F\text{F}F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$$$G\text{G}G=\left[\begin{array}{c}0.5\Delta t^2\\ \Delta t\end{array}\right]$$ $$u_n\text{un}{u}_n=(a_n+g)$$

协方差外推方程

协方差外推方程的一般形式为：$$P_{n+1,n}=F{P}_{n,n}{F}^T+Q\text{Pn+1,n=FPn,nFT+Q}{P}_{n+1,n}=F{P}_{n,n}{F}^T+Q$$其中：
$P_{n,n}\text{Pn,n}{P}_{n,n}$ 是当前状态的估计不确定性（协方差）矩阵
$P_{n+1,n}\text{Pn+1,n}{P}_{n+1,n}$ 是在当前预测的下一个状态的估计不确定性（协方差）矩阵
$F\text{F}F$ 是之前在“线性动态系统模型”部分推导出来的状态转移矩阵
$Q\text{Q}Q$ 是过程噪声矩阵

估计不确定性的矩阵形式为：$$P\text{P}P=\left[\begin{array}{cc}{p}_x& p_{x\dot{x}}\\ p_{\dot{x}x}& p_{\dot{x}}\end{array}\right]$$
矩阵对角线上的元素是估计值的方差：
$\bullet$ $p_x$ 是高度估计的方差
$\bullet$ $p_{\dot{x}}$ 是速度估计的方差
$\bullet$ 非对角线上的元素是协方差

过程噪声矩阵

这里假设它是离散噪声模型：不同时间段的噪声是不同的，在一个时间段里面是恒定的。

恒定加速度的过程噪声矩阵如下：$$Q\text{Q}Q=\left[\begin{array}{cc}{\sigma }_x^2& {\sigma }_{x\dot{x}}^2\\ {\sigma }_{\dot{x}x}^2& {\sigma }_x^2\end{array}\right]$$

前面已经推导了恒定加速度模型的Q矩阵，本例的Q矩阵为：$$Q\text{Q}Q=\left[\begin{array}{cc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}\\ \frac{\Delta t^3}{2}& \Delta t^2\end{array}\right]{ϵ}^2$$其中：
$\bullet$ $\Delta t$ 是连续测量之间的时间间隔
$\bullet$ ${ϵ}^2$ 是加速度计测量值的随机方差

在前面的例子里，我们使用了在加速度中的系统随机方差 ${\sigma }_a^2$ 作为过程噪声矩阵的乘数。但是在这里我们有加速度计，可以测量系统随机加速度。加速度计的误差v比系统随机加速度低得多，因此这里我们用 ${ϵ}^2$ 作为过程噪声的乘数。

它使得我们的估计不确定性更低！

现在可以写出我们例子的协方差外推矩阵：$$P_{n+1,n}=F{P}_{n,n}{F}^T+Q\text{Pn+1,n=FPn,nFT+Q}{P}_{n+1,n}=F{P}_{n,n}{F}^T+Q$$$$\left[\begin{array}{cc}{p}_{x_{n+1,n}}& p_{x{\dot{x}}_{n+1,n}}\\ p_{\dot{x}{x}_{n+1,n}}& p_{{\dot{x}}_{n+1,n}}\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\times \left[\begin{array}{cc}{p}_{x_{n,n}}& p_{x{\dot{x}}_{n,n}}\\ p_{\dot{x}{x}_{n,n}}& p_{{\dot{x}}_{n,n}}\end{array}\right]\times \left[\begin{array}{cc}1& 0\\ \Delta t& 1\end{array}\right]+\left[\begin{array}{cc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}\\ \frac{\Delta t^3}{2}& \Delta t^2\end{array}\right]{ϵ}^2$$

测量方程

矩阵形式的测量方程的一般形式为：$$z_n\text{zn}{z}_n=H\text{H}H{x}_n\text{xn}{x}_n+v_n\text{vn}{v}_n$$其中：
$z_n\text{zn}{z}_n$ 是测量向量（measurement vector）
$x_n\text{xn}{x}_n$ 是真实系统状态（隐藏状态）
$v_n\text{vn}{v}_n$ 是随机噪声向量
$H\text{H}H$ 是观察矩阵(observation matrix)

我们得到的测量值只有火箭的高度：$z_n\text{zn}{z}_n=\left[\begin{array}{c}{x}_{n,measured}\end{array}\right]$ $$z_n\text{zn}{z}_n=H\text{H}H{x}_n\text{xn}{x}_n$$$$[x_{n,measured}]=H\text{H}H\left[\begin{array}{c}{x}_n\\ {\dot{x}}_n\end{array}\right]$$

$z_n\text{zn}{z}_n$ 的维度是 $1\times 1$，$x_n\text{xn}{x}_n$ 的维度是 $2\times 1$， 因此观测矩阵 $H\text{H}H$ 的维度应该为 $1\times 2$ $$H\text{H}H=\left[\begin{array}{cc}1& 0\end{array}\right]$$

测量不确定性

测量协方差矩阵为：$$R_n\text{Rn}{R}_n=\left[\begin{array}{c}{\sigma }_{x_m}^2\end{array}\right]$$ 下标 $m$ 用于测量不确定性。

为了简化例子，假设测量不确定性是恒定的：$$R_1\text{R1}{R}_1=R_2\text{R2}{R}_2\dots R_{n-1}\text{Rn−1}{R}_{n-1}=R_n\text{Rn}{R}_n=R\text{R}R$$

卡尔曼增益

矩阵形式的卡尔曼增益如下：$$K_n\text{Kn}{K}_n=P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}{H\text{H}H}^T({H{P}_{n,n-1}H\text{HPn,n−1H}H{P}_{n,n-1}H}^T+R_n\text{Rn}{R}_n{)}^{-1}$$其中：
$K_n\text{Kn}{K}_n$ 是卡尔曼增益
$P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}$ 是在当前时刻的先前估计不确定性（方差）矩阵（在上一个状态预测得到的）
$H\text{H}H$ 是观察矩阵
$R_n\text{Rn}{R}_n$ 是测量不确定性（测量噪声协方差矩阵）

前面已经导出了卡尔曼增益所需要的元素：$$\left[\begin{array}{c}{K}_{1,1_n}\\ K_{2,1_n}\end{array}\right]=\left[\begin{array}{cc}{p}_{x_{n,n-1}}& p_{x{\dot{x}}_{n,n-1}}\\ p_{\dot{x}{x}_{n,n-1}}& p_{{\dot{x}}_{n,n-1}}\end{array}\right]\left[\begin{array}{c}1\\ 0\end{array}\right]\times {\left(\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{cc}{p}_{x_{n,n-1}}& p_{x{\dot{x}}_{n,n-1}}\\ p_{\dot{x}{x}_{n,n-1}}& p_{{\dot{x}}_{n,n-1}}\end{array}\right]\left[\begin{array}{c}1\\ 0\end{array}\right]+[{\sigma }_{x_m}^2]\right)}^{-1}$$

状态更新方程

状态更新方程的矩阵形式为：$${\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}={\hat{x}}_{n,n-1}\text{x^n,n−1}{\hat{x}}_{n,n-1}+K_n\text{Kn}{K}_n(z_n\text{zn}{z}_n-H\text{H}H{\hat{x}}_{n,n-1}\text{x^n,n−1}{\hat{x}}_{n,n-1})$$其中：
${\hat{x}}_{n,n}\text{x^n,n}{\hat{x}}_{n,n}$ 是在时刻 $n$ 的估计系统状态向量
${\hat{x}}_{n,n-1}\text{x^n,n−1}{\hat{x}}_{n,n-1}$ 是在时刻 $n-1$ 的预测系统状态向量
$K_n\text{Kn}{K}_n$ 是卡尔曼增益
$z_n\text{zn}{z}_n$ 是测量值
$H\text{H}H$ 是观察矩阵(observation matrix)

构成状态更新方程的元素已经在前面全部定义好了。

协方差更新方程

$$P_{n,n}\text{Pn,n}{P}_{n,n}=(I\text{I}I-K_{n}H\text{KnH}{K}_{n}H)P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}(I\text{I}I-K_{n}H\text{KnH}{K}_{n}H{)}^T+K_n\text{Kn}{K}_n{R}_n\text{Rn}{R}_n{K_n\text{Kn}{K}_n}^T$$ 其中
$P_{n,n}\text{Pn,n}{P}_{n,n}$ 是当前状态的估计不确定性(协方差)矩阵
$P_{n,n-1}\text{Pn,n−1}{P}_{n,n-1}$ 是在当前状态下，先前估计不确定性(协方差)矩阵 （在上一个状态时预测得到的）
$K_n\text{Kn}{K}_n$ 是卡尔曼增益
$H\text{H}H$ 是观察矩阵
$R_n\text{Rn}{R}_n$ 是测量不确定性（测量噪声协方差矩阵）

构成协方差更新方程的所有元素已经在前面全部定义好了。

数值实例

假设火箭以恒定加速度垂直助推，火箭上安装了加速度计，可以提供高度的测量值和加速度的测量值，作为卡尔曼滤波器的控制输入。

$\bullet$ 测量时间间隔为 $\Delta t=0.25s$
$\bullet$ 火箭加速度 $\ddot{x}=30\frac{m}{s^2}$
$\bullet$ 高度的测量误差标准差：${\sigma }_{x_m}=20m$
$\bullet$ 加速度计的测量误差标准差：$ϵ=0.1\frac{m}{s^2}$
$\bullet$ 状态过渡矩阵 $F\text{F}F$ 为：$$F\text{F}F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}1& 0.25\\ 0& 1\end{array}\right]$$
$\bullet$ 控制矩阵 $B\text{B}B$ 为：$$G\text{G}G=\left[\begin{array}{c}0.5\Delta t^2\\ \Delta t\end{array}\right]=\left[\begin{array}{c}0.0313\\ 0.25\end{array}\right]$$
$\bullet$ 过程噪声矩阵 $Q\text{Q}Q$ 为：$$Q\text{Q}Q=\left[\begin{array}{cc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}\\ \frac{\Delta t^3}{2}& \Delta t^2\end{array}\right]{\sigma }_a^2=\left[\begin{array}{cc}\frac{0.25^4}{4}& \frac{0.25^3}{2}\\ \frac{0.25^3}{2}& 0.25^2\end{array}\right]0.1^2$$

$\bullet$ 测量不确定性 $R\text{R}R$ 为：$$R_n\text{Rn}{R}_n=R\text{R}R=[{\sigma }_{x_m}^2]=400$$

下面的表格是 30 次含噪声的测量值：

		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
高度	$x_n(m)$	-32.4	-11.1	18	22.9	19.5	28.6	46.5	68.9	48.2	56.1	90.5	104.9	140.9	148	187.6	209.2	244.6	276.4	323.5	357.3	357.4	398.3	446.7	465.1	529.4	570.4	636.8	693.3	707.3	748.5
加速度	$a_n(\frac{m}{s^2})$	39.72	40.02	39.97	39.81	39.75	39.6	39.77	39.83	39.73	39.87	39.81	39.92	39.78	39.98	39.76	39.86	39.61	39.86	39.74	39.87	39.63	39.67	39.96	39.8	39.89	39.85	39.9	39.81	39.81	39.68

第 0 次迭代

初始化

我们不知道火箭的位置；初始位置和速度都设为 $0$：

$${\hat{x}}_{0,0}\text{x^0,0}{\hat{x}}_{0,0}=\left[\begin{array}{c}0\\ 0\end{array}\right]$$

火箭的加速度也不知道，但是我们可以推测它会比 $0$ 大，所以假设：

$$u_0\text{u0}{u}_0=g$$

由于初始状态向量是个猜测值，我们将会设置很高的估计不确定性。高的估计不确定性会导致高卡尔曼增益，给到测量值很高的权重。$$P_{0,0}\text{P0,0}{P}_{0,0}=\left[\begin{array}{cc}500& 0\\ 0& 500\end{array}\right]$$

预测

现在我们可以基于初始值预测下一个状态：$$\begin{array}{rl}{\hat{x}}_{1,0}\text{x^1,0}{\hat{x}}_{1,0}& =F\text{F}F{\hat{x}}_{0,0}\text{x^0,0}{\hat{x}}_{0,0}+G\text{G}G{u}_0\text{u0}{u}_0\\ & =\left[\begin{array}{cc}1& 0.25\\ 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 0\end{array}\right]+\left[\begin{array}{c}0.0313\\ 0.25\end{array}\right]9.8\\ & =\left[\begin{array}{c}0.3\\ 2.45\end{array}\right]\end{array}$$

$$\begin{array}{rl}{P}_{1,0}\text{P1,0}{P}_{1,0}& =F\text{F}F{P}_{0,0}\text{P0,0}{P}_{0,0}{F\text{F}F}^T+Q\text{Q}Q\\ & =\left[\begin{array}{cc}1& 0.25\\ 0& 1\end{array}\right]\left[\begin{array}{cc}500& 0\\ 0& 500\end{array}\right]\left[\begin{array}{cc}1& 0\\ 0.25& 1\end{array}\right]+\left[\begin{array}{cc}\frac{0.25^4}{4}& \frac{0.25^3}{2}\\ \frac{0.25^3}{2}& 0.25^2\end{array}\right]0.1^2\\ & =\left[\begin{array}{cc}531.25& 125\\ 125& 500\end{array}\right]\end{array}$$

第 1 次迭代

步骤1 - 测量

测量值：$$\begin{gathered} z_1\text{z1}{z}_1=-32.40 \\ {u}_1\text{u1}{u}_1=39.72 \end{gathered}$$

步骤2 - 更新

卡尔曼增益的计算：$$K_1\text{K1}{K}_1=P_{1,0}\text{P1,0}{P}_{1,0}{H\text{H}H}^T(H\text{H}H{P}_{1,0}\text{P1,0}{P}_{1,0}{H\text{H}H}^T+R\text{R}R{)}^{-1}=\left[\begin{array}{c}0.57\\ 0.13\end{array}\right]$$

估计当前状态：$${\hat{x}}_{1,1}\text{x^1,1}{\hat{x}}_{1,1}={\hat{x}}_{1,0}\text{x^1,0}{\hat{x}}_{1,0}+K_1\text{K1}{K}_1(z_1\text{z1}{z}_1-H\text{H}H{\hat{x}}_{1,0}\text{x^1,0}{\hat{x}}_{1,0})=\left[\begin{array}{c}-18.35\\ -1.94\end{array}\right]$$

更新当前估计不确定性：$$P_{1,1}\text{P1,1}{P}_{1,1}=(I\text{I}I-K_{1}H\text{K1H}{K}_{1}H)P_{1,0}\text{P1,0}{P}_{1,0}(I\text{I}I-K_{1}H\text{K1H}{K}_{1}H{)}^T+K_1\text{K1}{K}_{1}R\text{R}R{K_1\text{K1}{K}_1}^T=\left[\begin{array}{cc}228.2& 53.7\\ 53.7& 483.2\end{array}\right]$$

步骤3 - 预测

$$\begin{array}{rl}{\hat{x}}_{2,1}\text{x^2,1}{\hat{x}}_{2,1}& =F\text{F}F{\hat{x}}_{1,1}\text{x^1,1}{\hat{x}}_{1,1}+G\text{G}G{u}_1\text{u1}{u}_1\\ & =\left[\begin{array}{cc}1& 0.25\\ 0& 1\end{array}\right]\left[\begin{array}{c}-18.35\\ 0.25\end{array}\right]+\left[\begin{array}{c}0.0313\\ 0.25\end{array}\right]\left(39.72+(-9.8)\right)\\ & =\left[\begin{array}{c}-17.9\\ 5.54\end{array}\right]\end{array}$$

$$P_{2,1}\text{P2,1}{P}_{2,1}=F\text{F}F{P}_{1,1}\text{P1,1}{P}_{1,1}{F\text{F}F}^T+Q\text{Q}Q=\left[\begin{array}{cc}285.2& 174.5\\ 174.5& 483.2\end{array}\right]$$

预测不确定性仍然非常高。

第 2 次迭代

步骤1 - 测量

测量值：$$\begin{gathered} z_2\text{z2}{z}_2=-11.1 \\ {u}_2\text{u2}{u}_2=40.02 \end{gathered}$$

步骤2 - 更新

卡尔曼增益的计算：$$K_2\text{K2}{K}_2=P_{2,1}\text{P2,1}{P}_{2,1}{H\text{H}H}^T(H\text{H}H{P}_{2,1}\text{P2,1}{P}_{2,1}{H\text{H}H}^T+R\text{R}R{)}^{-1}=\left[\begin{array}{c}0.42\\ 0.26\end{array}\right]$$

估计当前状态：$${\hat{x}}_{2,2}\text{x^2,2}{\hat{x}}_{2,2}={\hat{x}}_{2,1}\text{x^2,1}{\hat{x}}_{2,1}+K_2\text{K2}{K}_2(z_2\text{z2}{z}_2-H\text{H}H{\hat{x}}_{2,1}\text{x^2,1}{\hat{x}}_{2,1})=\left[\begin{array}{c}-15.1\\ 7.3\end{array}\right]$$

更新当前估计不确定性：$$P_{2,2}\text{P2,2}{P}_{2,2}=(I\text{I}I-K_{2}H\text{K2H}{K}_{2}H)P_{2,1}\text{P2,1}{P}_{2,1}(I\text{I}I-K_{2}H\text{K2H}{K}_{2}H{)}^T+K_2\text{K2}{K}_{2}R\text{R}R{K_2\text{K2}{K}_2}^T=\left[\begin{array}{cc}166.5& 101.9\\ 101.9& 438.8\end{array}\right]$$

步骤3 - 预测

$${\hat{x}}_{3,2}\text{x^3,2}{\hat{x}}_{3,2}=F\text{F}F{\hat{x}}_{2,2}\text{x^2,2}{\hat{x}}_{2,2}+G\text{G}G{u}_2\text{u2}{u}_2=\left[\begin{array}{c}-12.3\\ 14.8\end{array}\right]$$

$$P_{3,2}\text{P3,2}{P}_{3,2}=F\text{F}F{P}_{2,2}\text{P2,2}{P}_{2,2}{F\text{F}F}^T+Q\text{Q}Q=\left[\begin{array}{cc}244.9& 211.6\\ 211.6& 438.8\end{array}\right]$$

现在到最后一轮迭代看看。

第 30 次迭代

步骤1 - 测量

测量值：$$\begin{gathered} z_{30}\text{z30}{z}_{30}=748.35 \\ {u}_2\text{u2}{u}_2=39.68 \end{gathered}$$

步骤2 - 更新

卡尔曼增益的计算：$$K_{30}\text{K30}{K}_{30}=P_{30,29}\text{P30,29}{P}_{30,29}{H\text{H}H}^T(H\text{H}H{P}_{30,29}\text{P30,29}{P}_{30,29}{H\text{H}H}^T+R\text{R}R{)}^{-1}=\left[\begin{array}{c}0.12\\ 0.02\end{array}\right]$$

高度值的卡尔曼增益汇聚到 $0.12$ ，意味着估计值的权重要比测量值的权重要高。

估计当前状态：$${\hat{x}}_{30,30}\text{x^30,30}{\hat{x}}_{30,30}={\hat{x}}_{30,29}\text{x^30,29}{\hat{x}}_{30,29}+K_{30}\text{K30}{K}_{30}(z_{30}\text{z30}{z}_{30}-H\text{H}H{\hat{x}}_{30,29}\text{x^30,29}{\hat{x}}_{30,29})=\left[\begin{array}{c}776.7\\ 215.4\end{array}\right]$$

更新当前估计不确定性：$$P_{30,30}\text{P30,30}{P}_{30,30}=(I\text{I}I-K_{30}H\text{K30H}{K}_{30}H)P_{30,29}\text{P30,29}{P}_{30,29}(I\text{I}I-K_{30}H\text{K30H}{K}_{30}H{)}^T+K_{30}\text{K30}{K}_{30}R\text{R}R{K_{30}\text{K30}{K}_{30}}^T=\left[\begin{array}{cc}49.3& 9.7\\ 9.7& 2.6\end{array}\right]$$

此时高度的不确定性 $p_x=49.3$，说明预测值的标准差为 $\sqrt{49.3}m=7.02m$。（测量值的标准差是 $20m$）

步骤3 - 预测

$${\hat{x}}_{31,30}\text{x^31,30}{\hat{x}}_{31,30}=F\text{F}F{\hat{x}}_{30,30}\text{x^30,30}{\hat{x}}_{30,30}+G\text{G}G{u}_{30}\text{u30}{u}_{30}=\left[\begin{array}{c}831.5\\ 222.94\end{array}\right]$$

$$P_{31,30}\text{P31,30}{P}_{31,30}=F\text{F}F{P}_{30,30}\text{P30,30}{P}_{30,30}{F\text{F}F}^T+Q\text{Q}Q=\left[\begin{array}{cc}54.3& 10.4\\ 10.4& 2.6\end{array}\right]$$

本例总结

下面的表格是火箭高度的真实值，测量值，估计值：

卡尔曼增益：

开始的时候，估计的高度会受到测量的影响，并且由于测量的噪声很大，因此与实际的火箭高度并不一致。

但随着Kalman增益的收敛，噪声测量的影响较小，估计的高度与真实高度吻合良好。

在这个例子中，我们没有任何操作导致加速度的改变，但是如果我们有，控制输入（加速计）将更新状态外推方程。