想厘清卡尔曼滤波，就必须要讲清一些最基本的 “文字游戏”。观测值 是测量得到的，物体实际属性为 真实值 或者叫 实际值。但是这个地方要注意的是，观测值 也是 “准确的”，我们把测量的误差单独拿出来一项作为 噪音。所以那些系数矩阵（物理理论关系）也是 “准确的”，所有的误差均归结于 噪音。如果说你认为模型不准确等等，那些因素就称为 错误；错误 往往会带来 定值偏差。一般情况下，不考虑 错误。所以，去除噪音的物理属性称为 理想值或者叫 理论值。因为我们经常谈到理想状态下，也就是没有噪音的理论值公式。而在导航领域是必须要合理考虑影响结果精度的一些关键 噪音 的。

数学导航

卡尔曼滤波

参考教程

【从放弃到精通！卡尔曼滤波从理论到实践~】：从该教程中提取公式并理解。这个讲得很好，但是公式不够严谨，还有小错误，而且完全跳过了重要的更新过程。
【【不要再看那些过时的卡尔曼滤波老教程了】2022巨献，卡尔曼滤波-目标追踪从理论到实战最新版全套教程！建议收藏】：只看了p1而且讲得很不好，但是帮助理解更细那块有点点小用吧。
一文看懂卡尔曼滤波（附全网最详细公式推导） - 付聪的文章 - 知乎：公式推导挺全面的，但是流畅度还是不如我自己的这篇。

基本滤波知识

状态空间表达式

状态方程

状态方程 如下：

\[ x_t = F \cdot x_{t-1} + B \cdot u_{t-1} + m_t \]

解释：当前时刻的状态 \(x_t\) 等于上一时刻的状态 \(x_{t-1}\) 乘以系数 \(F\)，加上输入值 \(u_t\) 乘以系数 \(B\)，最后加上 过程噪声 \(m_t\)。

\(F\) 又称为 状态转移矩阵，因为 \(F\) 的大小决定了状态变化的快慢。\(B\) 又称为控制矩阵，因为 \(B\) 的大小决定了输入对系统影响的程度。

如何理解过程噪声 \(m_t\)？状态方程的含义是我们对系统的数学建模，构建了一个数学表达式，但是实际的系统往往是复杂的，在随时间变化的时候，会受到各种各样因素的干扰，可以认为是一个总干扰，这种干扰是随机的，符合正太分布（高斯分布）。

观测方程

观测方程 如下：

\[ z_t = H \cdot x_t + n_t \]

\(z_t\) 是观测量，\(x_t\) 是状态量，\(H\) 是状态量的系数，又称 观测矩阵 因为我们假定观测量和状态量之间的关系是线性的，可以由这个线性矩阵 \(H\) 表示。\(n_t\) 是 观测噪声。

可以这样理解：测量值 \(z_t\) 和状态量 \(x_t\) 存在一种线性关系，但是测量总是有误差的，所以要加上观测噪声 \(n_t\)，观测噪声同样符合正太分布。

我们可以用一个方框图表示这个系统：

参数分析

噪音服从均值为零的正态分布，可以记为公式 噪音服从正态分布：

\[ \begin{aligned} m_t \in N(0,Q) \\ n_t \in N(0,R) \end{aligned} \]

他们的均值均为零，方差分别为 \(Q\) 和 \(R\)，方差为常数，与时间无关。\(m_t\) 和 \(n_t\) 就是其中的一个取值，只是取得这个结果的概率服从正态分布，用公式 噪音服从正态分布的表达式 表示：

\[ \begin{aligned} f (m_t) = \frac {1} {\sqrt{2 \cdot \pi \cdot Q}} \cdot e^{ - \frac {m_t^2} {2 \cdot Q} } \\ f (n_t) = \frac {1} {\sqrt{2 \cdot \pi \cdot R}} \cdot e^{ - \frac {n_t^2} {2 \cdot R} } \end{aligned} \]

超参数

噪音的方差，具体是多少，我们是不知道的，是需要不断调节的参数，从而使系统达到稳定。和调节PID一样，用公式 卡尔曼滤波和PID调参 表示：

\[ Q,R \sim PID \]

卡尔曼滤波的理解

通过卡尔曼滤波也不会得到真实值，而是每个时刻状态的 最优估计值 \(\widehat{x}_t\)，是修正值，也叫 后验估计值。总结来说是，上一时刻的 最优估计值 \(\widehat{x}_{t-1}\)，根据物理理论再加上过程噪声（状态方程）得到的 先验估计值 \(\hat{x}_t^{-}\)，然后利用有噪音的观测值（观测方程） \(z_t\)，进行加权得到最优估计值 \(\widehat{x}_t\)。这样从先验到后验，也就实现了 预测到更新，而最优估计值的不断变化，也就是 迭代。从先验估计的正态分布和观测值的正态分布的两个波峰到后验（最优）估计正态分布的一个波峰的变化，也就是实现了 滤波。前后对比方差减小了，也就是 去除了部分噪音。后验估计值居于先验估计值和观测值之间，更加接近真实值，所以这是一种 加权滤波。可以通过观察图解：一阶简单演示来理解上述表述。

通俗公式理解

预测模型

考虑一个情景，一辆匀加速（加速度为 \(a\)）直线运动的小车行驶在公路上，我们研究小车的状态有位置 \(p\) 和速度 \(v\)，根据物理规律构建状态方程，用公式 匀加速直线运动1 表示：

\[ \begin{aligned} &p_t = p_{t-1} + v_{t-1}\cdot\Delta t + \frac {a}2\cdot\Delta t^2 \\ &v_t = v_{t-1} + a\cdot\Delta t \end{aligned} \]

其中 \(t\) 表示当前时刻，\(t - 1\) 表示上一时刻。这是高中物理学习的标准的位置和速度公式。写成矩阵形式，用公式 匀加速直线运动2 表示：

\[ \begin{bmatrix} p_t \\ v_t \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \cdot\begin{bmatrix} p_{t-1} \\ v_{t-1} \end{bmatrix} + \begin{bmatrix} \frac{\Delta t^2}2 \\ \Delta t \end{bmatrix} \cdot a \]

写成矩阵的形式，用公式 理想的状态方程 表示：

\[ \underline{x} = F \cdot \underline{x_{t-1}} + B \cdot u_{t-1} \]

其中下划线 \(\underline{x}\) 表示没用噪音的理想值。上式加上噪音后，用公式 过程噪音 表示：

\[ \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} r_t(p) \\ r_t(v) \end{bmatrix} \]

满足状态方程。其中，\(r_t(p)\) 是位置噪音，\(r_t(v)\) 是速度噪音，均服从正太分布，噪音是随机的，与时间有关，每个时刻产生随机的噪音。

预测方程

状态预测

依据去除噪音的状态方程，就能直接写出先验估计的表达式，用公式 先验估计 表示：

\[ \hat{x}_{t}^{-} = F \cdot \hat{x}_{t-1} + B \cdot u_{t-1} \]

符号 \(\hat{x}_{t}^{-}\) 和 \(\hat{x}_{t-1}\) 的含义在前文卡尔曼滤波直观图解中已经提到，引入符号 \(\hat{}\) 和\(^{-}\) 是为了表示估计和先验。

状态误差预测

上式先验估计对比状态方程，可以求出 先验误差 \({e}_{t}^{-}\)，用公式 先验误差 表示：

\[ {e}_{t}^{-} = x_{t} - \hat{x}_{t}^{-} = F \cdot (x_{t-1} - \hat{x}_{t-1}) + m_t \]

先验误差表示真实值和预测值的差值。其中\(x_{t} - \hat{x}_{t}\)为 后验误差 \({e}_t\)，用公式 后验误差 表示：

\[ {e}_t = x_{t} - \hat{x}_{t} \]

后验误差表示真实值和估计值的差值。所以上式先验误差可以用公式 匀加速直线运动2 进一步表示为：

\[ {e}_{t}^{-} = F \cdot e_{t-1} + m_t \]

其中误差 \(m_t\) 是个随机值，我们能利用的就是它服从正态分布，均值为零没有利用价值，方差为常数 \(Q\) ，所以按道理来说两边求方差，但是考虑到这不可能不是一个一维的，而是多维的矩阵，我们要求每个元素的方差，所以这里求协方差，我们只用对角线的元素，因为协方差矩阵的对角线的元素是每个变量的方差，具体可以参考：协方差。那么求先验误差的协方差矩阵，用公式 先验误差的协方差矩阵1 表示：

\[ \begin{aligned} Cov({e}_{t}^{-},{e}_{t}^{-}) &= Cov({e}_{t}^{-}) \\ &= Cov(F \cdot e_{t-1} + m_t) \\ &= F \cdot Cov(e_{t-1}) \cdot F^{T} + Cov(m_t) \\ \end{aligned} \]

记 先验误差的协方差矩阵 \(Cov({e}_{t}^{-}) = P_{t}^{-}\)，则上式可以用公式 先验误差的协方差矩阵2 表示：

\[ P_{t}^{-} = F \cdot P_{t-1} \cdot F^{T} + Q \]

其中 \(P_{t-1}\) 是上一时刻的后验误差的协方差矩阵。不同状态之间的噪音很可能不是独立的，他们具有一定的相关性。

更新模型

对于这个小车，我们有卫星测量，卫星只能直接测量小车的位置 \(z_p\)，卫星无法直接测量速度 \(z_v\)，所以理想的位置和速度观测方程中不带速度分量，可以用公式 位置和速度观测方程 表示：

\[ \begin{aligned} z_p &= p_t \end{aligned} \]

写成矩阵的形式，用公式 位置和速度观测方程的矩阵形式 表示：

\[ \begin{bmatrix} z_p \end{bmatrix} = \begin{bmatrix} 1 & 0 \end{bmatrix} \cdot\begin{bmatrix} p_t \\ v_t \end{bmatrix} \]

一定不能写成下面的形式，因为 \(z_v\) 没有被观测，而不是 \(z_v = 0\)，用公式 错误的位置和速度观测方程 表示：

\[ \begin{aligned} z_p &= p_t \\ z_v &= 0 \end{aligned} \]

错误的矩阵的形式，用公式 错误的位置和速度观测方程的矩阵形式 表示：

\[ \begin{bmatrix} z_p \\ z_v \end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 0 \end{bmatrix} \cdot\begin{bmatrix} p_t \\ v_t \end{bmatrix} \]

其中引入了速度观测值 \(v_t\)，但是速度是没有测量的，可以去掉这个测量量。同样的有其他传感器的话可以增加测量量。所以观测方程的维度可以和状态方程的维度不同。写成矩阵形式，用公式 匀加速直线运动2 表示：

\[ \underline{z_t} = H \cdot \underline{x_t} \]

上式加上以下噪音，用公式 观测噪音 表示：

\[ \begin{bmatrix} 1 & 0 \end{bmatrix}\cdot \begin{bmatrix} \Delta p_t \\ \Delta v_t \end{bmatrix} \]

其中，理想值 \(\underline{z_t}\) 和实际值 \(z_t\) 之间存在位置观测噪音 \(\Delta p_t\)，速度观测噪音 \(\Delta v_t\)。可以看到该位置和速度观测方程的矩阵形式加上噪音后，满足观测方程。

更新方程

修正估计

先验估计值是理想状态下，没有噪音，是根据物理理论规律推导出来的当前时刻的预测的状态值 \(\hat{x}_t^-\)。如果我们把观测方程也去除噪音值，当作一个理想的公式，并把这个先验估计值 \(\hat{x}_t^-\) 带入去除噪音的观测方程中，那么实际的观测值和理论上理想的观测值之间存在一个差值，这个值就叫 测量残差，也叫 残差，记为 \(l_t\)，用公式 残差 表示：

\[ l_t = z_t - H \cdot \hat{x}_t^- \]

我们永远无法得到状态的真实值 \(x_t\)，只能尽量得到一个最优的估计值 \(\hat{x}_t\)，但是这个值无法直接用先验估计值 \(\hat{x}_t^-\) 和残差直接表示，因为残差是理想状态计算的观测值和实际的观测值之间的差值，但是这个值并不能直接表示后验估计值 \(\hat{x}_t\) 和先验估计值 \(\hat{x}_t^-\) 之间的差值，但是可以用一个系数 \(K_t\) 使他们相等，用公式 最优估计偏差和残差的关系 表示：

\[ \hat{x}_t - \hat{x}_t^- = K_t \cdot (z_t - H \cdot \hat{x}_t^-) \]

其中 \(K_t\) 又称为 卡尔曼滤波系数，上式我们习惯用公式 最优估计值计算公式 写成：

\[ \hat{x}_t = \hat{x}_t^- + K_t \cdot (z_t-H \cdot \hat{x}_t^-) \]

这样，最优估计值 \(\hat{x}_t\) 等于当前时刻的预测值 \(\hat{x}_t^-\)，加上权重 \(K_t\)乘以观测误差 \(z_t-H \cdot \hat{x}_t^-\)。

更新卡尔曼增益

那么这个权重 \(K_t\) 给多少呢？怎么给呢？是随便给吗？有什么依据吗？

答：当然是有依据的。依据当然是真实值和最优估计值之间的差值 \({e}_t\) 最小，即后验误差最小，后验误差见公式后验误差。将上式最优估计值带入公式后验误差中，用公式 后验误差变换1 表示：

\[ \begin{aligned} {e}_t = x_{t} - [\hat{x}_t^- + K_t \cdot (z_t - H \cdot \hat{x}_t^-)] \end{aligned} \]

继续化简，将观测方程带入上式中，用公式 后验误差变换2 表示：

\[ \begin{aligned} {e}_t &= x_{t} - [\hat{x}_t^- + K_t \cdot (H \cdot x_{t} + n_t - H \cdot \hat{x}_t^-)] \\ &= (x_{t} - \hat{x}_t^-) - K_t \cdot H \cdot (x_{t} - \hat{x}_t^-) - K_t \cdot n_t \end{aligned} \]

继续化简，将公式先验误差 \({e}_{t}^{-}\) 带入上式中，用公式 后验误差变换3 表示：

\[ \begin{aligned} {e}_t &= (x_{t} - \hat{x}_t^-) - K_t \cdot H \cdot (x_{t} - \hat{x}_t^-) - K_t \cdot n_t \\ &= {e}_{t}^{-} - K_t \cdot H \cdot {e}_{t}^{-} - K_t \cdot n_t \\ &= (I - K_t \cdot H) \cdot {e}_{t}^{-} - K_t \cdot n_t \end{aligned} \]

同理，接下来求后验误差的协方差矩阵，用公式 后验误差的协方差矩阵1 表示：

\[ \begin{aligned} Cov({e}_{t},{e}_{t}) &= Cov({e}_{t}) \\ &= Cov[(I - K_t \cdot H) \cdot {e}_{t}^{-} - K_t \cdot n_t] \\ &= (I - K_t \cdot H) \cdot Cov({e}_{t}^{-}) \cdot (I - K_t \cdot H)^T + K_t \cdot Cov(n_t) \cdot K_t^T \end{aligned} \]

记 后验误差的协方差矩阵 \(Cov({e}_{t}) = P_{t}\)，则上式可以用公式 后验误差的协方差矩阵2 表示：

\[ \begin{aligned} P_{t} &= (I - K_t \cdot H) \cdot P_{t}^{-} \cdot (I - K_t \cdot H)^T + K_t \cdot R \cdot K_t^T \\ &= (P_{t}^{-} - K_t \cdot H \cdot P_{t}^{-}) \cdot (I - H^T \cdot K_t^T) + K_t \cdot R \cdot K_t^T \\ &= P_{t}^{-} - K_t \cdot H \cdot P_{t}^{-} - P_{t}^{-} \cdot H^T \cdot K_t^T \\ &+ K_t \cdot H \cdot P_{t}^{-} \cdot H^T \cdot K_t^T + K_t \cdot R \cdot K_t^T \\ &= P_{t}^{-} - K_t \cdot H \cdot P_{t}^{-} - (K_t \cdot H \cdot P_{t}^{-})^T + K_t \cdot (H \cdot P_{t}^{-} \cdot H^T + R) \cdot K_t^T \end{aligned} \]

其中之所以能化简，是由于误差的协方差矩阵 \(P_{t}\) 和 \(P_{t}^-\) 均是自相关矩阵，即以对角线为分界线，矩阵是对称的，这是协方差矩阵的性质，具体可以参考：协方差。前面提到我们只用协方差对角线的元素，我们想要后协方差矩阵 \(P_{t}\) 对角线上的每个元素都最小，只要让该矩阵对角线上的和最小就行了，即矩阵 \(P_{t}\) 的迹最小（参考：矩阵的迹），用公式 后验误差的协方差矩阵的迹 表示：

\[ \begin{aligned} T(P_{t}) &= T(P_{t}^{-}) - 2 \cdot T(K_t \cdot H \cdot P_{t}^{-}) + T[K_t \cdot (H \cdot P_{t}^{-} \cdot H^T + R) \cdot K_t^T] \\ &= T[K_t \cdot (H \cdot P_{t}^{-} \cdot H^T + R) \cdot K_t^T] - 2 \cdot T(K_t \cdot H \cdot P_{t}^{-}) + T(P_{t}^{-}) \\ \end{aligned} \]

其中 \(T()\) 代表矩阵的迹。把上式看作是关于 \(K_t\) 的函数，用公式 关于卡尔曼滤波系数的函数 表示：

\[ \begin{aligned} f(x) &= a \cdot x^2 - b \cdot x + c, \quad(a,b\text{是正的常数}) \\ f'(x) &= 2 \cdot a \cdot x - b \end{aligned} \]

显然，这个函数是一个开口向上的一元二次函数，有极小值。它是一个凸函数，导数为0的点就是最小值。将公式后验误差的协方差矩阵的迹求导，因为和的求导等于导的求和，所以矩阵迹的求导同矩阵求导求导用公式 后验误差的协方差矩阵的迹的转置 表示：

\[ \begin{aligned} {\frac {dT(P_{t})} {dK_{t}}} &= T\left[K_{t} \cdot \left(H \cdot P_{t}^{-} \cdot H^{T} + R \right)\right] + T\left[K_{t} \cdot (H \cdot P_{t}^{-} \cdot H^{T} + R)\right] - T\left[2 \cdot (H \cdot P_{t}^{-})^{T}\right] \\ &= T\left[K_{t} \cdot \left(H \cdot P_{t}^{-} \cdot H^{T} + R \right)\right] -2 \cdot T\left[(H \cdot P_{t}^{-})^{T}\right] \\ &= 0 \end{aligned} \]

这里是对矩阵的求导，求导公式从函数求导理解，想完全弄清楚，那么具体矩阵的求导需要单独学习，这里不做深入探讨。使得上式为零，等式左右两边矩阵的迹相等使得矩阵相等即可。那么可计算出卡尔曼滤波系数 \(K_{t}\)，用公式 卡尔曼滤波系数 表示：

\[ K_t = \frac {P_t^- \cdot H^T} {H \cdot P_t^- \cdot H^T + R} \]

更新后验误差的协方差

将计算得到的卡尔曼滤波系数 \(K_t\) 带入公式后验误差的协方差矩阵2，计算得到后验误差协方差矩阵，用公式 后验误差的协方差矩阵3 表示：

\[ \begin{aligned} P_{t} &= P_{t}^{-} - K_t \cdot H \cdot P_{t}^{-} \\ &= (I - K_t \cdot H) \cdot P_{t}^{-} \\ \end{aligned} \]

总结

至此，卡尔曼滤波的五个重要的公式已经全部推导完成。分别是：先验估计、先验误差的协方差矩阵2、卡尔曼滤波系数、最优估计值计算公式、后验误差的协方差矩阵3。综上，我们把这五个重要的公式写在一起：

预测公式，用公式 预测 表示：

\[ \begin{aligned} \hat{x}_{t}^{-} &= F \cdot \hat{x}_{t-1} + B \cdot u_{t-1} \\ P_{t}^{-} &= F \cdot P_{t-1} \cdot F^{T} + Q \end{aligned} \]

含义为：根据系统的物理模型，使用上一时刻的状态估计和控制输入，预测系统的下一时刻状态和状态方差。

更新公式，用公式 更新 表示：

\[ \begin{aligned} K_t &= \frac {P_t^- \cdot H^T} {H \cdot P_t^- \cdot H^T + R} \\ \hat{x}_t &= \hat{x}_t^- + K_t \cdot (z_t - H \cdot \hat{x}_t^-) \\ P_{t} &= (I - K_t \cdot H) \cdot P_{t}^{-} \end{aligned} \]

含义为：通过比较系统的观测值和预测值，结合观测噪声和系统模型的不确定性，更新系统的状态估计和方差。

上面没有说误差的方差而是直接说的是 "先验估计" \(\hat{x}_{t}^{-}\) 和 "后验估计" \(\hat{x}_t\) 的方差，是因为虽然我们不知道真实值 \(x_t\) （不可测也是不可观的），但是它是个常数，由方差的性质知，变量增加或者减少一个常数，其方差值不变，所以这个误差方差就是该估计值的正太分布的方差。至于为什么直接说方差而不是协方差矩阵，上文已经提及了。

一轮简单迭代计算

对于上述小车，只考虑状态量位置 \(p\)，初始位置 \(p_0 = 0\)，初始速度 \(v_0 = 0\)，加速度 \(a_0 = 1\)。则易得：\(F = 1\)、\(B = 0.5\)、\(u = 1\)。假设卫星直接测量得到小车的位置，即 \(H = 1\)。初始值：\(\hat{x}_{0}^{-} = 0\)、规定给零\(P_{0} = 0\)。给定带误差的观测值：\(z_t = 1\)，误差比较大。

则进行第一轮计算：\(\hat{x}_{1}^{-} = 0.5\)、\(P_{1}^{-} = Q\)、\(K_1 = Q/(Q +R)\)、\(\hat{x}_1 = 0.5 + 0.5 \cdot Q/(Q +R)\)、\(P_{1} = (Q \cdot R)/(Q +R)\)。实际值 \(x_1 = 0.5\)，可以看到我们模拟的是系统的物理模型建立很准确，所以先验估值 \(\hat{x}_{1}^{-}\) 很准确，其可信度高，我们调参就是把状态误差的方差 \(Q\) 调小，取值为 \(0.005\)：而观测值误差很大，其可信度低，我们调参就是把观测误差的方差 \(R\) 调大，取值为 \(0.1\)。则 \(P_{1}^{-} = 0.005\)、\(K_1 = 0.0476\)、\(\hat{x}_1 = 0.524\)、\(P_{1} = 0.00476\)。仔细观察，我们可以发现有趣的现象，用公式 方差比较 表示为：

\[ \begin{aligned} Q - P_1 = Q - \frac {Q \cdot R} {Q + R} = \frac {Q^2} {Q + R} > 0 \\ R - P_1 = R - \frac {Q \cdot R} {Q + R} = \frac {R^2} {Q + R} > 0 \end{aligned} \]

从上式可以看出，最优估计的方差是比过程噪音和观测噪音都小的。这也符合我们的预期。

一阶简单演示

我们使用 geogebra 软件做演示。由于部分符号不支持，这里做替换。数学模型还是选用一维小车匀加速直线运动：

给定值：\(F = 1\)、\(B = 0.5\)、\(H = 1\)、\(u = 1\)、\(P_{0-hou} = 0\)、\(x_{0-hou} = 0\)

数值滑块：\(Q\)、\(R\)。

第一轮：

观测值的正态分布（红色曲线）得先给，以此来创造滑动点 \(Guan\)（绿色点），取它的横坐标 \(z_1 = x(Guan)\) 作为观测值。想要得到观测值的正态分布需要先给真实值 \(x_{1-zhen}\)。之所以将 \(x_{1}\) 记为 \(x_{1-zhen}\)，是为了 Latex 公式直接运用于 geogebra 绘图软件中。用公式 geogebra观测值的正态分布公式 表示为：

\[ \begin{aligned} f_{1-guan}(x) = \frac {1} {\sqrt{2 \cdot \pi} \cdot \sqrt{R}} \cdot e^{ - \frac {(x - x_{1-zhen})^2} {2 \cdot R} } \end{aligned} \]

迭代公式用 geogebra卡尔曼滤波公式 表示为：

\[ \begin{aligned} x_{1-xian} = F \cdot x_{0-hou} + B \cdot u \end{aligned} \]

\[ \begin{aligned} P_{1-xian} = F^2 \cdot P_{0-hou} + Q \end{aligned} \]

\[ \begin{aligned} K_1 = \frac {P_{1-xian} \cdot H} {H^2 \cdot P_{1-xian} + R} \end{aligned} \]

\[ \begin{aligned} x_{1-hou} = x_{1-xian} + K_1 \cdot (z_1 - H \cdot x_{1-xian}) \end{aligned} \]

\[ \begin{aligned} P_{1-hou} = (1 - K_1 \cdot H) \cdot P_{1-xian} \end{aligned} \]

正态分布公式：

先验估计的正态分布曲线用蓝色表示，公式用 geogebra先验估计的正态分布公式 表示为：

\[ \begin{aligned} f_{1-xian}(x) = \frac {1} {\sqrt{2 \cdot \pi} \cdot \sqrt{P_{1-xian}}} \cdot e^{ - \frac {(x - x_{1-xian})^2} {2 \cdot P_{1-xian}} } \end{aligned} \]

后验估计的正态分布曲线用紫色表示，公式用 geogebra后验估计的正态分布公式 表示为：

\[ \begin{aligned} f_{1-hou}(x) = \frac {1} {\sqrt{2 \cdot \pi} \cdot \sqrt{P_{1-hou}}} \cdot e^{ - \frac {(x - x_{1-hou})^2} {2 \cdot P_{1-hou}} } \end{aligned} \]

注意复制粘贴到 geogebra 中时不能有对齐符号 "&"。演示结果如下：

通过图解调节参数，可以大大增加我们对卡尔曼滤波原理的理解。其中有很重要的地方需要说明。1. 是状态模型更可靠还是观测模型更可靠，我们就把哪个模型对应的权重调大。虽然在我们认识卡尔曼滤波之前会认为，过程噪音和观测噪音的方差是系统固有的，而我们需要去给权重提高某个模型的可靠度。而卡尔曼滤波和这个表面的、固式的思维恰好是相反的，权重是通过计算得到的，不用我们手动给，只要依照误差方差最小的原则计算出来即可。而过程噪音和观测噪音才是我们要调节的参数，也就是我们要调节的权重，方差给的越小，说明离散程度越小，说明越可靠。也就是说：如果该物理模型更准确，那么我们就把过程噪音的方差调小；如果传感器的精度很高，我们就把观测噪音方差调小。经过反复调节参数，我们发现最优估计曲线（迭代后的）和真实轨迹吻合度很好时，此时的方差参数往往就和物理系统实际的方差参数几乎一致。2. 调节参数时，我们把观测值远离真实值，最优估计值也会跟着远离真实值，但是明明还受到了先验估计值的约束，而此时最优估计的效果不好。简单分析会发现此时说明观测模型不准确了，应该降低其权重，所以将观测噪音方差调大，最优估计值会更信任先验估计值，那么可能此时的最优估计效果才更好。

多次迭代仿真

所有的代码算法，严格按照公式推导进行设计。所有的代码变量全部取自于latex公式符号。这样接口就做好了，这样逻辑思维的一致性，严谨性，可读性，方便性，记忆性，代码的移植性都得到了极大地提升。有两点需要额外设计：一是初始值，这是在公式推导中没有提及的。二是遇到了数值分析问题，需要对公式进行移项等基本变化进行改造；但也是要详细列写公式的，并严格依据公式写代码；数值问题是在跑代码时发现的，发现数值问题，再回头先改公式，再改代码。

代码只是一种机器表达语言，核心还是算法。代码需要分解成合适的文件、函数方便调用。重复的功能一定要做成函数，既能提升开发效率，又能方便维护。分解成合适的文件便于阅读和使用。不要在某个语言上写任何偏门的语法，尽量都写通用的语法，方便移植。

显然，我想分得更细、更合理一些。这次准备采用的语言有：matlba、C、C++、Python、Java、Verilog 来写。先写，然后进行函数化，然后进行文件化。比较好画图的是 Java、Python、matlab，所以仿真出来的数组还是要做成数据库保存下来，用其他编程语言作图。

例子还是用这个车，但是公式还是用通用的状态方程和过程方程，不用特例的力学方程。我们观察发现，真实值的方程和实际值的方程只差一个噪音，所以函数的功能就可以做成一个调用关系。初始值：

小车的初始速度：小车的初始加速度：位置的过程噪音，速度的过程噪音，传感器的观测噪音

我们先来看看 matlab。

matlab仿真

%% 清理
close all; clear; clc;
% 估计小车在每一时刻的位置和速度

%% 无噪音的理想值
T = (1 : 100); % 离散的时间序列，单位是s
p = zeros(1, length(T)); v = zeros(1, length(T)); 
p(1) = 0 % 小车的初始位置
v(1) = 3 % 小车的初始速度
a = 0.1; % 加速度

for t = 2 : length(T)
    p(t) = p(t - 1) + v(t - 1) + 1/2 * a; % 理想的位置
    v(t) = v(t - 1) + a; % 理想的速度值
end

underline_x = [p; v];
 
%% 有噪音的实际值，模拟噪音
% 状态噪音
sigma_r = 0.01
r_p = sigma_r * randn(1, length(T));
r_v = sigma_r * randn(1, length(T));
x = underline_x;
for t = 2 : length(T)
    x(1, t) = x(1, t - 1) + x(2, t - 1) + 1/2 * a + r_p(t); % 真实的位置
    x(2, t) = x(2, t - 1) + a + r_v(t); % 真实的速度值
end

% 观测噪音
sigma_Delta_p = 3
Delta_p = sigma_Delta_p * randn(1, length(T)); % GPS测量误差，标准差为3m
z = x(1, :) + Delta_p; % GPS的观测值，带有测量误差

%% 卡尔曼滤波
% 已知量
% 已知的线性变换矩阵
F = [1, 1; 0, 1]; % 状态转移矩阵
B = [1/2; 1]; % 控制矩阵
u = a;
H = [1, 0];
sigma_Q = 0.01
Q = [sigma_Q^2, 0; 0, sigma_Q^2]; % 过程噪声的协方差矩阵，这是一个超参数
sigma_R = 3
R = sigma_R^2; % 观测噪声的协方差矩阵，也是一个超参数。因为是一维的，就是一个数


% 初始化
hat_x_ = zeros(2, length(T)); hat_x = zeros(2, length(T));
hat_x(:, 1) = [0; 0]; % 初始状态，[位置, 速度]，就是我们要估计的内容，开始时都未知，设为0
P = [0.1, 0; 0, 0.1]; % 先验误差协方差矩阵的初始值，根据经验给出

% 卡尔曼滤波5个关键公式
for t = 2:length(T)
    hat_x_(:, t) = F * hat_x(:, t - 1) + B * u;
    P_ = F * P * F' + Q;
    K = (P_ * H') / (H * P_ * H' + R);
    hat_x(:, t) = hat_x_(:, t) + K * (z(:, t) - H * hat_x_(:, t));
    P = (eye(2) - K * H) * P_;
end

%% 绘图
figure(1);
plot(T, underline_x(1, :), 'b'); % 位置的理想值
hold on;
plot(T, x(1, :), 'g'); % 位置的实际值
plot(T, z(1, :), 'm'); % 位置的观测值
plot(T, hat_x(1, :), 'r.'); % 位置的最优估计值
title('对位置的估计');
xlabel('时间'); ylabel('位置');

figure(2);
plot(T, underline_x(2, :)); % 速度的理想值
hold on;
plot(T, x(2, :), 'g'); % 速度的实际值
plot(T, hat_x(2, :), 'r.'); % 速度的最优估计值
title('对速度的估计');
xlabel('时间'); ylabel('速度');

其中，用matlab做实时脚本时，这些地方可以做数字滑块：1. 初始位置 p(1) 和速度 v(1)，2. 过程噪声的标准差 sigma_r 和观测噪声的标准差 sigma_Delta_p，3. 超参数 \(Q\) 和 \(R\) 的标准差 sigma_Q 和 sigma_R。

不管初始位置和速度时多少，卡尔曼滤波很快就能收敛，跟上脚步，和实际值接近。过程噪声的标准差不能很大，很大说明状态方程的物理模型建立的不合理，且在没有观测速度的条件下，卡尔曼滤波效果特别是对速度的估计值就很差；除非观测噪声很小，观测精度很高，卡尔曼滤波在状态方程的物理模型建立的不合理的条件下，依赖观测值也能得到较好的结果。超参数 \(Q\) 和 \(R\) 可能需要不断地调试，以达到最佳的曲线效果；可以发现把超参数的标准差和模拟噪音的标准差设置一样时效果最好；实际中由于对过程噪声的未知，需要通过不断调试更改超参数，以使得曲线最佳，此时的超参数基本上就和系统的噪音相差无几。

时间复杂度

需要考虑matlab仿真的时间复杂度。就是考虑算力问题。时间复杂度想得到的答案是随着数据规模的增加，程序的运行次数是线性增加还是指数增加的。从这里我们不难看出，数据规模有三个量，一个是状态量的维度 \(m\)，一个是观测量的维度 \(n\)，一个是数组的长度 \(t\)。显然循环停止的条件是数组跑完，所以时间复杂度肯定包含 \(\cdot t\) 项。接下来就只用考虑单步预测的时间复杂度：显然不管计算了多少次，复杂度只和维度有关，所以，用公式 时间复杂度 表示：

\[ \begin{aligned} T &= O(m + n \cdot t) \\ T &= O(n^2) \end{aligned} \]

这是对的吗？应该是对的吧？

GNSS定位

变换

以上推导的公式形式常用于自动控制领域。在卫星导航领域，状态空间表达式如下，用公式 状态空间表达式 表示：

\[ \left\{ \begin{aligned} X_k &= \Phi_k \cdot X_{k-1} + \psi_{k,k-1} \cdot U_{k-1} + \Gamma_{k,k-1} \cdot \Omega_{k-1} \\ L_k &= B_k \cdot X_k + G_k \cdot U_k + \Delta_k \end{aligned} \right. \]

上式和状态方程以及观测方程还有三个区别：

一个是下角标 \(_{k,k-1}\) 出现在一些系数矩阵上，这是为什么呢？是因为系数矩阵所乘以的矩阵变量是上一时刻的，从上一时刻到这一个时刻的矩阵变换关系表示为 \({k,k-1}\)。第二个变化是观测方程的单噪音项变成了两项相乘，其中 \({\Omega}_{k-1}\) 为上一时刻的动态噪声，\({\Gamma}_{k,k-1}\) 为噪音变化矩阵。第三个变化是在观测方程中，该时刻的控制项 \(G_k \cdot U_k\)，该控制项将影响观测值，在部分传感器中会出现这种观测方程，这种传感器我目前还没见过。

同理，可以得到卡尔曼滤波公式：

时间更新，用公式 时间更新 表示：

\[ \left\{ \begin{aligned} \widehat{X}_{k,k-1} &= \Phi_{k,k-1} \cdot \widehat{X}_{k-1,k-1} + \psi_{k,k-1} \cdot U_{k-1} \\ D_{X_{k,k-1}} &= \Phi_{k,k-1} \cdot D_{X_{k-1,k-1}} \cdot \Phi_{k,k-1}^{T} + \Gamma_{k,k-1} \cdot D_{\Omega_{k-1}} \cdot \Gamma_{k,k-1}^{T} \end{aligned} \right. \]

上式与公式预测除了符号以外完全相同，但是确实不够优雅，可读性也不如预测，比如先验误差符号有点怪怪的。之前叫预测，但是这里叫一步预测公式的时间更新，因为从公式上来说，是从上一个时刻推导到这时刻。但是预测的叫法和时间更新的叫法值得是一个东西。

测量更新，用公式 测量更新1 表示：

\[ \begin{aligned} \widehat{X}_{k,k} &= \widehat{X}_{k,k-1} + J_k \cdot l_k \\ D_{X_{k,k}} &= (I - J_k \cdot B_k) \cdot D_{X_{k,k-1}} \end{aligned} \]

其中，用公式 测量更新2 表示：

\[ \begin{aligned} J_k &= D_{X_{k,k-1}} \cdot B_k^T\big(B_k \cdot D_{X_{k,k-1}} \cdot B_k^T + D_{\Delta_k}\big)^{-1} \\ l_k &= L_k - Z_k - B_k \cdot \widehat{X}_{k,k-1} \end{aligned} \]

其中，用公式 非随机控制向量 表示：

\[ Z_{k} = G_{k} \cdot U_{k} \]

上式和更新方程除了符号以及状态方程那儿说的不同之处以外没有任何不同。这里 \(J_k\) 除了之前叫卡尔曼滤波系数，这里还叫滤波增益矩阵。\(l_k\) 除了叫验前残差以外还叫新息矩阵、OMC向量。