让GNSS&RTK不再难【第15讲 Kalman滤波】

第15讲 Kalman滤波

15.1 一个简单的小例子

我们要每隔十分钟测量室内的温度，正好我们手中有一个温度计，其测量精度为0.1°C。最简单的做法是每隔十分钟用温度计测一次，然后发布出去，每次发布的温度的精度为0.1°C，可以满足我们的日常需要。

但是随着温度计使用年限过久，其精度有大幅下降，每测量一次的随机误差在1°C，也就是每次测量的结果符合标准差为1°C的正态分布。如果还是将每次温度计的读数直接发布，那可能会对我们的日常生活带来影响，因为发布的温度精度有明显下降。

方法之一是，如果每次测量时同时有10个温度计，那么对10个结果取平均作为最后结果，那么可以提升最后结果的精度。

如果我们仅有一个温度计，如何挖掘其他有用信息，然后提升我们的最后的精度呢？

我们知道温度不可能忽然变化，一般十分钟前和现在的温度基本上一样。这是一个很有用的信息，即使我们这个时刻没有温度计的读数，如果我们有十分钟前的温度信息，那么我们就可以将十分钟前的温度结果拿过来，直接作为当前温度发布出去。

这样不就不需要温度计了吗？

每次我们都可以用十分钟前的温度结果，这样一直外推下去不就好了？

好像哪里不对。

其实这样一直外推也没问题，只是我们在关注结果同时，需要加上结果的精度信息。一个小时前测的温度32°C，其精度为1°C，由此我们外推得到十分钟前的温度为32°C，但其精度有所变差，变为1.5°C。由此类推，一个小时后我依然发布当前的温度为32°C，只是其精度为6°C。

有问题吗？没有问题。

15.2 Kalman滤波算法公式

Kalman滤波算法分为两个阶段：

1. 预测阶段：卡尔曼滤波器使用历史的估计值估计当前时刻的状态和方差信息；
2. 更新阶段：使用当前时刻的观测更新预测的状态和方差信息。

这两个阶段交替迭代包含了时间更新和测量更新两个重要部分。

首先以一个离散线性控制过程为例，该系统能够用一个线性随机微分方程描述（即系统状态方程）：

$${\hat{x}}_{k|k-1}=H{\hat{x}}_{k-1}+B{u}_{k-1}+W_{k-1}$$
其中：

• $H$ ——状态转移矩阵；
• $B$ ——控制矩阵；
• $u_{k-1}$ ——k时刻的系统控制变量；
• $W_{k-1}$ ——系统噪声向量，为高斯白噪声，协方差为 $Q$。

再加上系统的测量值（即观测方程）：
$$z_k=A{x}_k+V_k$$
其中：

• $A$ ——系统测量矩阵；
• $V_k$ ——测量噪声向量，为高斯白噪声，协方差为 $R$；
• $z_k$ ——k时刻的测量值，但即对系统的部分观测向量。对于系统状态变量而言，并不一定能观测所有的量。

整个系统更新思路包括：状态更新和协方差更新。

Kalman滤波的五个关键公式：

时间更新

$${\hat{x}}_{k|k-1}=H{\hat{x}}_{k-1}+B{u}_{k-1}\text{(1)}$$

$$P_{k|k-1}=H{P}_{k-1}{H}^T+Q\text{(2)}$$

公式(1)和(2)表示对系统的预测值的计算；

观测更新

$$K_k=P_{k|k-1}{A}^T(A{P}_{k|k-1}{A}^T+R{)}^{-1}\text{(3)}$$

$${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-A{\hat{x}}_{k|k-1})\text{(4)}$$

公式(3)和(4)表示对系统的测量更新；

但是为了使卡尔曼滤波器不断的运行下去直到系统过程结束，需要更新k状态下 ${\hat{x}}_k$ 的协方差：
$$P_k=(I-K_{k}A)P_{k|k-1}\text{(5)}$$
其中 $I$ 为单位矩阵。

当系统进入k+1状态时， $P_k$ 就是公式(2)的 $P_{k-1}$。这样，算法就可以自回归的运算下去。

运行过程：(1)(2)-(3)-(4)(5)-(1)(2)……

输出量： ${\hat{x}}_k$ ——k时刻系统状态最优估计值和其协方差 $P_k$。

15.3 Kalman滤波的温度实时计算

接下来就需要将外推值，与当前历元的观测值进行融合，得到最终的温度结果。
简单理一下：

1. 我们在0时刻测了一个温度为32℃，其误差即为温度计的测量精度1℃。由于没有历史温度信息，本次发布为，温度：32℃，精度1℃。

2. 在10分钟这个时刻，温度计读数为33℃，同样测量精度为1℃；但我们有十分钟前的外推结果，外推温度为32℃，精度为1.5℃（1℃+0.5℃），其中0.5℃为我们外推的精度损失。对于两个温度结果，我们使用加权平均的方式来计算当前实际的温度。在此，我们引入Kalman滤波公式。

在本例子中，状态量 ${\hat{x}}_k$ 就一个，就是当前时刻的温度。状态转移矩阵H也很简单，就是1。
公式(1)(2)的预测过程已经完成。只是我们上面给出的是标准差，公式需要用方差。

使用公式(3)计算kalman增益，即
$$K_k=\frac{1.5^2}{1.5^2+1^2}=0.6923$$

那么10分钟时刻的最优温度：

$${\hat{x}}_k=32+K_k\ast (33-32)=32.6923$$

其相应的方差信息

$$P_k=(1-K_k)\ast 1.5^2=0.6925$$

对其开根号，即当前时刻最后的温度精度为0.83℃，明显的该精度是优于仅使用温度计结果。
所以，我们发布10分钟时刻的温度和精度为32.69℃和0.83℃。

3. 在20分钟这个时刻，温度计读数为32℃，同样测量精度为1℃。上一个时刻的温度和精度为32.69℃和0.83℃，同样可以外推到当前历元的温度和精度信息。

15.4 PVA（位置、速度、加速度）状态转移模型

在RTKLIB中，rtk算法对Kalman滤波使用PVA状态转移模型，具体见 rtkpos.c -> udpos 函数。以下是单点的PVA状态转移函数，该函数逻辑与rtk算法一致，仅名称不同 (pntpos.c -> udpos_spp) 以避免对rtk算法的影响。

状态转移矩阵的代码逻辑

时间变化量 tt 为相邻历元的时间差，其关系如下：
$$P_1=P_0+V_0\cdot tt+\frac{1}{2}{a}_0\cdot t{t}^2$$

$$V_1=V_0+a_0\cdot tt$$

$$a_1=a_0$$

以下是状态转移矩阵的打印结果：
$$F=\left[\begin{array}{ccccccccc}1.000& 0.000& 0.000& 1.000& 0.000& 0.000& 0.500& 0.000& 0.000\\ 0.000& 1.000& 0.000& 0.000& 1.000& 0.000& 0.000& 0.500& 0.000\\ 0.000& 0.000& 1.000& 0.000& 0.000& 1.000& 0.000& 0.000& 0.500\\ 0.000& 0.000& 0.000& 1.000& 0.000& 0.000& 0.000& 0.000& 0.000\\ 0.000& 0.000& 0.000& 0.000& 1.000& 0.000& 0.000& 0.000& 0.000\\ 0.000& 0.000& 0.000& 0.000& 0.000& 1.000& 0.000& 0.000& 0.000\\ 0.000& 0.000& 0.000& 0.000& 0.000& 0.000& 1.000& 0.000& 0.000\\ 0.000& 0.000& 0.000& 0.000& 0.000& 0.000& 0.000& 1.000& 0.000\\ 0.000& 0.000& 0.000& 0.000& 0.000& 0.000& 0.000& 0.000& 1.000\end{array}\right]$$

状态转移矩阵F解析:

图片中的状态转移矩阵 $F$ 代表位置、速度、加速度在时间 $tt$ 后的关系。矩阵的具体形式如下：

$$F=\left[\begin{array}{ccccccccc}1& 0& 0& tt& 0& 0& 0.5t{t}^2& 0& 0\\ 0& 1& 0& 0& tt& 0& 0& 0.5t{t}^2& 0\\ 0& 0& 1& 0& 0& tt& 0& 0& 0.5t{t}^2\\ 0& 0& 0& 1& 0& 0& tt& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& tt& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& tt\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$

状态转移矩阵F展示了状态向量（位置、速度、加速度）在连续时间步间的变化规律，基于上述物理公式。具体解析为：

• 位置(P)：矩阵的前三行表示位置受前一时刻位置、速度及加速度影响的变化。
  • 位置P的更新规则:
    $$P_1=P_0+V_0\cdot tt+\frac{1}{2}{a}_0\cdot t{t}^2$$
  • 矩阵F中的体现:
    $P_1$的x分量由$P_0$的x分量、$v_0$的x分量乘以$tt$和$1/2a_0$的x分量乘以$tt{}^2$共同决定，对应的矩阵元素为[1,0,0,1,0,0,0.5,0,0]。
    同理，y和z分量也有相应的系数，因此整个矩阵的前三行（除了对角线外）反映了位置受速度和加速度影响的部分。
• 速度(V)：接下来的三行说明了速度如何基于前一状态及加速度更新。
  • 速度V的更新规则:
    $$a_1=a_0$$
  • 矩阵F中的体现:
    $V_1$的x分量由$V_0$的x分量和$a_0$ ​的x分量乘以$tt$决定，对应的矩阵元素为[0,0,0,0,1,0,0,0.5,0]。
    同样，y和z分量的更新规则也在矩阵的第四至第六行中体现，除了对角线上保持为1（表示速度的延续性）外，其余位置为0，直到速度受加速度影响的部分。
• 加速度(a)：最后三行体现了加速度的恒定性，假设无外力影响。
  • 加速度a的更新规则:
    公式:
    $$a_1=a_0$$
  • 矩阵F中的体现:
    加速度在理想情况下是恒定的，不随时间变化，因此在状态转移中，加速度的各分量保持不变，矩阵的第七至第九行对角线上的元素均为1，表示加速度状态的自保持，其余元素为0。

另一个需要注意，我们只需要在加速度上加入过程噪声，即放大预测值的方差信息。因为从状态转移矩阵中，我们就可以知道，加速度的精度信息会传递到速度和位置。
当然，你也可以修改任何状态量的方差信息，只要你认为该状态的精度符合你修改要求。不要去修改协方差信息，尤其是对于RTKLIB这种对速度和加速度没有测量更新的模型。

附加A：

【浅显易懂】卡尔曼滤波原理解读_哔哩哔哩_bilibili

卡尔曼滤波前言

简单平均:

首先，考虑两个温度测量值 $T_1$ 和 $T_2$，它们的真实值 $T$ 和测量误差 ${ϵ}_1$ 和 ${ϵ}_2$ 分别为：

$$T_1=T+{ϵ}_1$$

$$T_2=T+{ϵ}_2$$

其中， ${ϵ}_1\sim N(0,{\sigma }_1^2)$，${ϵ}_2\sim N(0,{\sigma }_2^2)$，且 $E[{ϵ}_1{ϵ}_2]=0$。

假设用简单平均法来估计 $T$：

$${\hat{T}}_{est1}=\frac{T_1+T_2}{2}$$

其方差为：

$$D[{\hat{T}}_{est1}]=D\left[\frac{T_1+T_2}{2}\right]$$

由于 $T_1$ 和 $T_2$ 是独立的，方差计算如下：

$$D[{\hat{T}}_{est1}]=\frac{D[T_1]+D[T_2]}{4}=\frac{{\sigma }_1^2+{\sigma }_2^2}{4}$$

加权平均:

现在考虑加权平均法：

$${\hat{T}}_{est2}=\alpha \cdot T_1+(1-\alpha )\cdot T_2$$

其方差为：

$$D[{\hat{T}}_{est2}]=D[\alpha \cdot T_1+(1-\alpha )\cdot T_2]$$

展开方差计算：

$$D[{\hat{T}}_{est2}]={\alpha }^{2}D[T_1]+(1-\alpha {)}^{2}D[T_2]$$

因此：

$$D[{\hat{T}}_{est2}]={\alpha }^2{\sigma }_1^2+(1-\alpha {)}^2{\sigma }_2^2$$

为了最小化方差，我们对 $\alpha$ 求导，并设导数为0，得到：

$$\frac{d}{d\alpha }\left({\alpha }^2{\sigma }_1^2+(1-\alpha {)}^2{\sigma }_2^2\right)=0$$

$$2\alpha {\sigma }_1^2-2(1-\alpha ){\sigma }_2^2=0$$

$$2\alpha {\sigma }_1^2=2{\sigma }_2^2-2\alpha {\sigma }_2^2$$

$$2\alpha {\sigma }_1^2+2\alpha {\sigma }_2^2=2{\sigma }_2^2$$

$$2\alpha ({\sigma }_1^2+{\sigma }_2^2)=2{\sigma }_2^2$$

$$\alpha ({\sigma }_1^2+{\sigma }_2^2)={\sigma }_2^2$$

$$\alpha =\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}$$

因此，最小化后的方差为：

min ⁡ { D [ T ^ e s t 2 ] } = α 2 σ 1 2 + ( 1 − α ) 2 σ 2 2 \min\{D[\hat{T}_{est2}]\} = \alpha^2 \sigma_1^2 + (1-\alpha)^2 \sigma_2^2 min{D[T^est2​]}=α2σ12​+(1−α)2σ22​

代入 $\alpha =\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}$：

$$={\left(\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)}^2{\sigma }_1^2+{\left(1-\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)}^2{\sigma }_2^2$$

$$=\frac{{\sigma }_1^2{\sigma }_2^4}{({\sigma }_1^2+{\sigma }_2^2{)}^2}+\frac{{\sigma }_1^4{\sigma }_2^2}{({\sigma }_1^2+{\sigma }_2^2{)}^2}$$

$$=\frac{{\sigma }_1^2{\sigma }_2^2({\sigma }_2^2+{\sigma }_1^2)}{({\sigma }_1^2+{\sigma }_2^2{)}^2}$$

$$=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}$$

基本不等式证明

我们可以证明，对于任意 $\alpha$，以下不等式成立：

$$\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\le \frac{{\sigma }_1^2+{\sigma }_2^2}{4}$$

为了证明这个不等式，我们考虑以下函数：

$$f(\alpha )={\alpha }^2{\sigma }_1^2+(1-\alpha {)}^2{\sigma }_2^2$$

首先，考虑 $\alpha =0.5$:

$$f(0.5)=(0.5{)}^2{\sigma }_1^2+(0.5{)}^2{\sigma }_2^2=0.25{\sigma }_1^2+0.25{\sigma }_2^2=0.25({\sigma }_1^2+{\sigma }_2^2)$$

我们需要证明：

$$\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\le 0.25({\sigma }_1^2+{\sigma }_2^2)$$

等价于证明：

$$4{\sigma }_1^2{\sigma }_2^2\le ({\sigma }_1^2+{\sigma }_2^2{)}^2$$

展开右侧：

$$({\sigma }_1^2+{\sigma }_2^2{)}^2={\sigma }_1^4+2{\sigma }_1^2{\sigma }_2^2+{\sigma }_2^4$$

因此，我们需要证明：

$$4{\sigma }_1^2{\sigma }_2^2\le {\sigma }_1^4+2{\sigma }_1^2{\sigma }_2^2+{\sigma }_2^4$$

即：

$$2{\sigma }_1^2{\sigma }_2^2\le {\sigma }_1^4+{\sigma }_2^4$$

这总是成立，因为 ${\sigma }_1^4+{\sigma }_2^4$ 永远大于或等于 $2{\sigma }_1^2{\sigma }_2^2$。可以通过以下方式进一步理解：

考虑以下恒等式：

$$({\sigma }_1^2-{\sigma }_2^2{)}^2\ge 0$$

展开后得到：

$${\sigma }_1^4-2{\sigma }_1^2{\sigma }_2^2+{\sigma }_2^4\ge 0$$

即：

$${\sigma }_1^4+{\sigma }_2^4\ge 2{\sigma }_1^2{\sigma }_2^2$$

因此，我们有：

$$2{\sigma }_1^2{\sigma }_2^2\le {\sigma }_1^4+{\sigma }_2^4$$

这证明了不等式：

$$\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\le \frac{{\sigma }_1^2+{\sigma }_2^2}{4}$$

因此，加权平均法的方差总是小于或等于简单平均法的方差。

公式解释

设:

$$T_1=T+{ϵ}_1$$

$$T_2=T+{ϵ}_2$$

其中:

$${ϵ}_1\sim N(0,{\sigma }_1^2),{ϵ}_2\sim N(0,{\sigma }_2^2),E[{ϵ}_1{ϵ}_2]=0$$

卡尔曼滤波思想

初值:
$${\hat{x}}_0,P_0$$

• ${\hat{x}}_0$: 初始状态向量的估计值。
• $P_0$: 初始状态估计误差的协方差矩阵。

系统参数:
$$A_k,B_k,Q_k,C_k,R_k$$

• $A_k$: 状态转移矩阵，描述系统从时刻 $k-1$ 到时刻 $k$ 的状态变化。
• $B_k$: 控制矩阵，描述控制输入 $u_k$ 对状态的影响。
• $Q_k$: 过程噪声协方差矩阵，描述过程噪声的统计特性。
• $C_k$: 观测矩阵，描述状态向量到观测向量的映射关系。
• $R_k$: 观测噪声协方差矩阵，描述观测噪声的统计特性。

预测:

1. 状态预测:

$${\hat{x}}_k^{\prime }=A_k{\hat{x}}_{k-1}+B_k{u}_k$$

• ${\hat{x}}_k^{\prime }$: 预测的状态向量。
• ${\hat{x}}_{k-1}$: 上一时刻的状态估计值。
• $u_k$: 当前时刻的控制输入。

2. 误差协方差预测:

$$P_k^{\prime }=A_k{P}_{k-1}{A}_k^T+Q_k$$

• $P_k^{\prime }$: 预测的误差协方差矩阵。
• $P_{k-1}$: 上一时刻的误差协方差矩阵。

更新:

3. 卡尔曼增益:

$$K_k=P_k^{\prime }{C}_k^T(C_k{P}_k^{\prime }{C}_k^T+R_k{)}^{-1}$$

• $K_k$: 卡尔曼增益矩阵，用于调整预测的状态向量。

4. 状态更新:

$${\hat{x}}_k={\hat{x}}_k^{\prime }+K_k(z_k-C_k{\hat{x}}_k^{\prime })$$

• ${\hat{x}}_k$: 更新后的状态向量。
• $z_k$: 当前时刻的观测值。

5. 误差协方差更新:

$$P_k=(I-K_k{C}_k)P_k^{\prime }$$

• $P_k$: 更新后的误差协方差矩阵。
• $I$: 单位矩阵。

卡尔曼滤波实际例子

初值:
$${\hat{x}}_0=\left[\begin{array}{c}0\\ 1\end{array}\right]$$
即小车起始位置为0，初始速度为1。

初始误差协方差矩阵:
$$P_0=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$

• 这是一个单位矩阵，表示在初始时刻，我们对位置和速度的估计都有单位方差（即初始的不确定性）。
• 每个元素的含义如下：
  • $P_{0_{11}}=1$：表示初始时刻位置估计的方差。
  • $P_{0_{22}}=1$：表示初始时刻速度估计的方差。
  • $P_{0_{12}}=P_{0_{21}}=0$：表示位置和速度之间的协方差为零，即它们在初始时刻是独立的。

系统参数:

• 状态转移矩阵 $A_k$:
  $$A_k=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$

  • 表示系统从时刻 $k-1$ 到时刻 $k$ 的状态转移。
  • $\Delta t$ 是时间间隔，第一行表示位置更新为前一时刻位置加上速度乘以时间，第二行表示速度保持不变。

  解释：

  • 矩阵 $A_k$ 由两个变量构成，分别是位置和速度。
  • 元素 $A_{11}=1$ 表示位置保持位置分量。
  • 元素 $A_{12}=\Delta t$ 表示时间间隔与速度的乘积对位置的影响。
  • 元素 $A_{21}=0$ 表示位置对速度没有直接影响。
  • 元素 $A_{22}=1$ 表示速度保持速度分量。

  具体来说，矩阵的第一行 $[1\Delta t]$ 表示位置更新的公式：
  $$x_k=x_{k-1}+v_{k-1}\Delta t$$

  矩阵的第二行 $[01]$ 表示速度保持不变：
  $$v_k=v_{k-1}$$

控制矩阵 $B_k$:
$$B_k=\left[\begin{array}{c}0.5\Delta t^2\\ \Delta t\end{array}\right]$$

• 表示控制输入对系统状态的影响。
• 第一行表示控制输入对位置的影响（加速度的一半乘以时间间隔的平方），即 $0.5\Delta t^2$。这是因为在物理学中，位置的改变不仅取决于初始速度，还包括加速度的影响，且加速度影响的位置变化量是 $0.5\cdot \text{加速度}\cdot (\Delta t{)}^2$。
• 第二行表示控制输入对速度的影响（加速度乘以时间间隔），即 $\Delta t$。在速度变化的公式中，速度的改变取决于加速度和时间间隔的乘积。

过程噪声协方差矩阵 $Q_k$:
$$Q_k=\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right]$$

• 表示过程噪声的统计特性。对位置和速度的过程噪声假设为0.1。

观测矩阵 $C_k$:
$$C_k=\left[\begin{array}{cc}1& 0\end{array}\right]$$

• 表示从状态向量到观测向量的映射关系。我们假设只能测量位置，所以观测矩阵表示只观察位置。

观测噪声协方差矩阵 $R_k$:
$$R_k=\left[\begin{array}{c}1\end{array}\right]$$

• 表示观测噪声的统计特性。假设观测噪声的方差为1。
• 该矩阵的形式表示在当前模型中，我们只考虑一个观测变量，并且假设观测噪声是一个具有方差为1的正态分布。
• 这个值反映了我们对测量精度的信心程度，方差越小，表示测量越精确。这里选择1表示我们假设观测噪声有一定的不确定性，但不是很大。

预测:

1. 状态预测:

$${\hat{x}}_k^{\prime }=A_k{\hat{x}}_{k-1}+B_k{u}_k$$

$${\hat{x}}_k^{\prime }=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ v_{k-1}\end{array}\right]+\left[\begin{array}{c}0.5\Delta t^2\\ \Delta t\end{array}\right]u_k$$

具体计算为：

$${\hat{x}}_k^{\prime }=\left[\begin{array}{c}{x}_{k-1}+v_{k-1}\Delta t+0.5u_k\Delta t^2\\ v_{k-1}+u_k\Delta t\end{array}\right]$$

2. 误差协方差预测:

$$P_k^{\prime }=A_k{P}_{k-1}{A}_k^T+Q_k$$

$$P_k^{\prime }=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]P_{k-1}\left[\begin{array}{cc}1& 0\\ \Delta t& 1\end{array}\right]+\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right]$$

具体计算为：

$$P_k^{\prime }=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{p}_{11}& p_{12}\\ p_{21}& p_{22}\end{array}\right]\left[\begin{array}{cc}1& 0\\ \Delta t& 1\end{array}\right]+\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right]$$

$$P_k^{\prime }=\left[\begin{array}{cc}{p}_{11}+\Delta t(p_{21}+p_{12})+\Delta t^2{p}_{22}& p_{12}+\Delta t{p}_{22}\\ p_{21}+\Delta t{p}_{22}& p_{22}\end{array}\right]+\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right]$$

$$P_k^{\prime }=\left[\begin{array}{cc}{p}_{11}+\Delta t(p_{21}+p_{12})+\Delta t^2{p}_{22}+0.1& p_{12}+\Delta t{p}_{22}\\ p_{21}+\Delta t{p}_{22}& p_{22}+0.1\end{array}\right]$$

更新:

3. 卡尔曼增益:

$$K_k=P_k^{\prime }{C}_k^T(C_k{P}_k^{\prime }{C}_k^T+R_k{)}^{-1}$$

$$K_k=P_k^{\prime }\left[\begin{array}{c}1\\ 0\end{array}\right]{\left(\left[\begin{array}{cc}1& 0\end{array}\right]P_k^{\prime }\left[\begin{array}{c}1\\ 0\end{array}\right]+1\right)}^{-1}$$

具体计算为：

$$K_k=\left[\begin{array}{c}{p}_{11}^{\prime }\\ p_{21}^{\prime }\end{array}\right]{\left(p_{11}^{\prime }+1\right)}^{-1}$$

4. 状态更新:

$${\hat{x}}_k={\hat{x}}_k^{\prime }+K_k(z_k-C_k{\hat{x}}_k^{\prime })$$

假设观测值 $z_k$ 是小车的实际位置，那么：

$${\hat{x}}_k={\hat{x}}_k^{\prime }+K_k(z_k-\left[\begin{array}{cc}1& 0\end{array}\right]{\hat{x}}_k^{\prime })$$

具体计算为：

$${\hat{x}}_k=\left[\begin{array}{c}{x}_{k-1}+v_{k-1}\Delta t+0.5u_k\Delta t^2\\ v_{k-1}+u_k\Delta t\end{array}\right]+K_k(z_k-x_{k-1}-v_{k-1}\Delta t-0.5u_k\Delta t^2)$$

5. 误差协方差更新:

$$P_k=(I-K_k{C}_k)P_k^{\prime }$$

$$P_k=(I-K_k\left[\begin{array}{cc}1& 0\end{array}\right])P_k^{\prime }$$

具体计算为：

$$P_k=(I-K_k\left[\begin{array}{cc}1& 0\end{array}\right])\left[\begin{array}{cc}{p}_{11}^{\prime }& p_{12}^{\prime }\\ p_{21}^{\prime }& p_{22}^{\prime }\end{array}\right]$$

通过以上步骤，我们完成了一次卡尔曼滤波的预测和更新过程，得到当前时刻 $k$ 的状态估计值 ${\hat{x}}_k$ 和误差协方差 $P_k$。

附加B：协方差矩阵

协方差矩阵的计算

假设我们有两个变量 $a$ 和 $b$，并且有 $m$ 个样本值。变量 $a$ 和 $b$ 的样本值分别为 $a_1,a_2,...,a_m$ 和 $b_1,b_2,...,b_m$。协方差矩阵可以通过以下步骤计算得到。

1. 定义矩阵 $X$：

$$X=\left[\begin{array}{cccc}{a}_1& a_2& \cdots & a_m\\ b_1& b_2& \cdots & b_m\end{array}\right]$$

2. 计算 $X$ 的转置矩阵 $X^T$：

$$X^T=\left[\begin{array}{cc}{a}_1& b_1\\ a_2& b_2\\ ⋮& ⋮\\ a_m& b_m\end{array}\right]$$

3. 计算协方差矩阵：

$$\frac{1}{m}X{X}^T=\frac{1}{m}\left[\begin{array}{cccc}{a}_1& a_2& \cdots & a_m\\ b_1& b_2& \cdots & b_m\end{array}\right]\left[\begin{array}{cc}{a}_1& b_1\\ a_2& b_2\\ ⋮& ⋮\\ a_m& b_m\end{array}\right]$$

4. 计算具体的协方差值：

$$=\left[\begin{array}{cc}\frac{1}{m}{\sum }_{i=1}^m{a}_i^2& \frac{1}{m}{\sum }_{i=1}^m{a}_i{b}_i\\ \frac{1}{m}{\sum }_{i=1}^m{a}_i{b}_i& \frac{1}{m}{\sum }_{i=1}^m{b}_i^2\end{array}\right]$$

5. 最终的协方差矩阵：

$$=\left[\begin{array}{cc}\text{Cov}(a,a)& \text{Cov}(a,b)\\ \text{Cov}(b,a)& \text{Cov}(b,b)\end{array}\right]$$

初始误差三维协方差矩阵示例

协方差矩阵展示了不同维度之间的方差和协方差。以三维数据 $x$，$y$ 和 $z$ 为例，协方差矩阵如下：

$$\left[\begin{array}{ccc}\text{Cov}(x,x)& \text{Cov}(x,y)& \text{Cov}(x,z)\\ \text{Cov}(y,x)& \text{Cov}(y,y)& \text{Cov}(y,z)\\ \text{Cov}(z,x)& \text{Cov}(z,y)& \text{Cov}(z,z)\end{array}\right]$$

• 方差: 对角线上的元素，如 $\text{Cov}(x,x)$, $\text{Cov}(y,y)$, 和 $\text{Cov}(z,z)$，表示变量 $x$，$y$ 和 $z$ 各自的方差。
• 协方差: 非对角线上的元素，如 $\text{Cov}(x,y)$ 和 $\text{Cov}(y,x)$，表示变量之间的协方差，反映它们之间的线性关系。

协方差矩阵解释

假设我们有一个物体在二维平面上移动，我们希望同时估计其位置（$x$ 和 $y$）和速度（$v_x$ 和 $v_y$）。初始误差协方差矩阵 $P_0$ 的形式为：

$$P_0=\left[\begin{array}{cccc}{\sigma }_x^2& {\sigma }_{xy}& {\sigma }_{x{v}_x}& {\sigma }_{x{v}_y}\\ {\sigma }_{yx}& {\sigma }_y^2& {\sigma }_{y{v}_x}& {\sigma }_{y{v}_y}\\ {\sigma }_{v_{x}x}& {\sigma }_{v_{x}y}& {\sigma }_{v_x}^2& {\sigma }_{v_x{v}_y}\\ {\sigma }_{v_{y}x}& {\sigma }_{v_{y}y}& {\sigma }_{v_y{v}_x}& {\sigma }_{v_y}^2\end{array}\right]$$

• ${\sigma }_x^2$ 和 ${\sigma }_y^2$ 表示位置 $x$ 和 $y$ 的方差，即对位置估计的不确定性。
• ${\sigma }_{v_x}^2$ 和 ${\sigma }_{v_y}^2$ 表示速度 $v_x$ 和 $v_y$ 的方差，即对速度估计的不确定性。
• ${\sigma }_{xy}$ 表示位置 $x$ 和 $y$ 之间的协方差，如果两个位置变量之间存在线性关系，则该值不为零。
• ${\sigma }_{x{v}_x}$ 和 ${\sigma }_{x{v}_y}$ 表示位置 $x$ 与速度 $v_x$ 和 $v_y$ 之间的协方差，表示位置和速度之间的关系。
• ${\sigma }_{v_x{v}_y}$ 表示速度 $v_x$ 和 $v_y$ 之间的协方差。

在初始情况下，假设位置和速度之间是独立的，并且在两个位置维度和两个速度维度之间没有相关性，矩阵可以简化为：

$$P_0=\left[\begin{array}{cccc}{\sigma }_x^2& 0& 0& 0\\ 0& {\sigma }_y^2& 0& 0\\ 0& 0& {\sigma }_{v_x}^2& 0\\ 0& 0& 0& {\sigma }_{v_y}^2\end{array}\right]$$

例如，如果 ${\sigma }_x^2=1$, ${\sigma }_y^2=1$, ${\sigma }_{v_x}^2=1$, ${\sigma }_{v_y}^2=1$，则：

$$P_0=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

这表示我们对初始位置和速度估计的方差都为1，并且位置和速度之间没有初始的相关性。