卡尔曼滤波讲解

一些基础知识

协方差系数与相关系数

相关系数是用于刻画两个变量或者序列的变化趋势是相同的还是相反的，也可以判断这个相同或者相反的程度。设变量序列X与Y，则协方差系数计算为

$C o v = E [(X - E (X)) (Y - E (Y))]$ $Cov = E[(X-E(X))(Y-E(Y))]$
如果2个变量是正相关的，则 $Cov>0$ ，否则 $Cov<0$ 。
相关系数的公式如下

$ρ = \frac{C o v}{σ_{X} σ_{Y}}$ $\rho = \frac{Cov}{\sigma_X \sigma_Y}$
因此相关系数的范围是 $-1 \le \rho \le 1$ 。
相关系数剔除了两个变量量纲的影响，可以被视为一种特殊的协方差系数；它更关注变量的变化趋势，而非其幅度信息。

参数估计

样本方差：样本方差的定义为 $E((X-E(X))^2)$ 。对于实际的变量序列X，其无偏的方差估计为

S^{2} = \frac{1}{n - 1} \sum_{i = 1}^{n} {(X_{i} - \bar{X})}^{2}

${S^2} = \frac{1}{{n - 1}}\sum\limits_{i = 1}^n {{{({X_i} - \bar X)}^2}}$

样本协方差：计算公式为
$S^{2} = \frac{1}{n - 1} \sum_{i = 1}^{n} (X_{i} - \bar{X}) (Y_{i} - \bar{Y})$ ${S^2} = \frac{1}{{n - 1}}\sum\limits_{i = 1}^n {({X_i} - \bar X)({Y_i} - \bar Y)}$

卡尔曼滤波

卡尔曼滤波是用于随机信号的算法，它是在特定的假设条件下，对测量结果与预测结果进行融合，得到最终的滤波结果。
在卡尔曼估计的过程中，只需要考虑测量噪声、估计噪声以及当前时刻状态系统的统计特性，因此计算量很小。

基本假设

后验概率 $P(x_{k-1} | y_{1:k-1})$ 为高斯分布
动态系统是线性的

$\begin{array}{l} x_{k} = A x_{k - 1} + B u_{k - 1} + q_{k - 1} \\ y_{k} = H x_{k} + r_{k} \end{array}$

其中
- $x(k)$ ：k时刻系统的状态（位置、速度等状态信息）。
- $u(k)$ ：对系统的控制量，如果是无控制系统，则系统方程中不包含这一项。
- $A, B$ ：系统参数（状态转移矩阵等）。
系统噪声与测量噪声都符合高斯分布，协方差矩阵为 $Q_{k-1}$ 与 $R_k$ 。

5个更新方程

这里写图片描述
* 首先赋予系统初始值，之后根据测量值就可以直接自回归地运算下去。

具体推导过程

以机器人移动为例，假设其有状态 $x_k$ ，表示位置与速度， $x_k = (p, v)$ ，可以通过GPS等信息得到其状态信息(传感器数据或者成为测量数据)，传感器数据有一个精度指标，但是我们可以通过后续的卡尔曼滤波操作，使得最终的状态信息的精度指标小于这个值。
假设对于单一变量，符号高斯分布的话，其均值为 $\mu$ ，方差为 $\sigma$ ，如果有多个变量，其协方差矩阵为 $\Sigma$ ，这里有个小的公式定理，如果
$C o v (x) = Σ$ $Cov(x) = \Sigma$
则有 $C o v (A x) = A Σ A^{T}$ $Cov(Ax) = A \Sigma A^T$ 。
假设机器人的状态转移矩阵为 $F_k$ ，协方差矩阵为 $P_k$ 。根据上面的公式有

$\begin{array}{l} {\hat{x}}_{k} = F_{k} {\hat{x}}_{k - 1} \\ P_{k} = F_{k} P_{k - 1} F_{k}^{T} \end{array}$ $\begin{array}{l} {{\hat x}_k} = {F_k}{{\hat x}_{k - 1}}\\ {P_k} = {F_k}{P_{k - 1}}F_k^T \end{array}$
上述公式中没有添加外部的控制变量，如果移动机器人可以通过油门或者刹车等部分进行加速度的控制，则可以给状态变量加入控制变量。假设知道了加速度a，则上式可以更新如下：

${\hat{x}}_{k} = F_{k} {\hat{x}}_{k - 1} + [\begin{array}{l} \frac{Δ t^{2}}{2} \\ Δ t \end{array}] a = F_{k} {\hat{x}}_{k - 1} + B_{k} u_{k}$ ${\hat x_k} = {F_k}{\hat x_{k - 1}} + \left[ \begin{array}{l} \frac{{\Delta {t^2}}}{2}\\ \Delta t \end{array} \right]a = {F_k}{\hat x_{k - 1}} + {B_k}{u_k}$
$B_k$ 即是系统的控制矩阵， $u_k$ 为控制向量。
上述公式均没有考虑到外界干扰，如果考虑到其他的如随机风力或者其他随机噪声的影响，可以将随机噪声视为符合高斯分布的，假设 $x_{k-1}$ 中的每个状态移动到一个新的符合高斯分布的区域，协方差为 $Q_k$ ，即相当于将没有被跟踪的干扰视为协方差为 $Q_k$ 的噪声。对协方差公式进行修正，得到下面的公式

$\begin{array}{l} {\hat{x}}_{k} = F_{k} {\hat{x}}_{k - 1} + B_{k} u_{k} \\ P_{k} = F_{k} P_{k - 1} F_{k}^{T} + Q_{k} \end{array}$ $\begin{array}{l} {{\hat x}_k} = {F_k}{{\hat x}_{k - 1}} + {B_k}{u_k}\\ {P_k} = {F_k}{P_{k - 1}}F_k^T + {Q_k} \end{array}$
上式刻画了一个较为完整的状态估计的方法：新的最优估计是根据上一最优估计的预测得到的，同时加上了外部控制变量的修正；新的不确定性是由上一不确定性的预测得到的，并且加上了外界环境的干扰。
上面只考虑到一个初步的预测估计，还需要结合传感器数据，得到更加准确的结果。
状态估计数据与传感器数据可能不一致，因此在统一的时候需要统一量纲，假设预测数据到传感器数据的转换矩阵为 $H_k$ ，则可以计算出预测数据的分布
$\begin{array}{l} μ_{expexcted} = H_{k} {\hat{x}}_{k} \\ Σ_{expexcted} = H_{k} P_{k} H_{k}^{T} \end{array}$ $\begin{array}{l} {\mu _{{\mathop{\rm expexcted}\nolimits} }} = {H_k}{{\hat x}_k}\\ {\Sigma _{{\mathop{\rm expexcted}\nolimits} }} = {H_k}{P_k}H_k^T \end{array}$
通过传感器或者状态估计的方法，均可以得到系统当前的预测的状态，但是由于均存在噪声，因此有些状态可能比单一预测的结果更接近真实状态。将传感器的不确定性用协方差 $R_k$ 表示，分布的均值就是读取得到的传感器数据或者测量值，记为 $z_k$ 。
引入一个小的公式定理：将两个高斯分布相乘，得到的结果仍然是符合高斯分布的(归一化之后)，新的均值与方差如下
$\begin{array}{l} μ^{'} = μ_{0} + \frac{σ_{0}^{2} (μ_{1} - μ_{0})}{σ_{0}^{2} + σ_{1}^{2}} \\ {σ^{'}}^{2} = σ_{0}^{2} - \frac{σ_{0}^{4}}{σ_{0}^{2} + σ_{1}^{2}} \end{array}$ $\begin{array}{l} \mu '{\rm{ = }}{\mu _{\rm{0}}}{\rm{ + }}\frac{{\sigma _0^2({\mu _1} - {\mu _0})}}{{\sigma _0^2 + \sigma _1^2}}\\ {{\sigma '}^2} = \sigma _0^2 - \frac{{\sigma _0^4}}{{\sigma _0^2 + \sigma _1^2}} \end{array}$

设 $k = \frac {\sigma_0^{2}} {{\sigma _0^2 + \sigma _1^2}}$
用矩阵表示，则有

\begin{array}{l} K = Σ_{0} (Σ_{0} + Σ_{1})^{- 1} \\ μ^{'} = μ_{0} + K (μ_{1} - μ_{0}) \\ Σ^{'} = Σ_{0} - K Σ_{0} \end{array}

$\begin{array}{l} K = {\Sigma _0}{({\Sigma _0} + {\Sigma _1})^{ - 1}}\\ \mu '{\rm{ = }}{\mu _{\rm{0}}}{\rm{ + K}}({\mu _1} - {\mu _0})\\ \Sigma ' = {\Sigma _0} - K{\Sigma _0} \end{array}$
将

K

$K$ 称为 卡尔曼增益
* 整合2个高斯分布，预测部分

\begin{array}{l} (μ_{0}, Σ_{0}) = (H_{k} {\hat{x}}_{k}, H_{k} P_{k} H_{k}^{T}) \\ (μ_{1}, Σ_{1}) = (z_{k}, R_{k}) \end{array}

$\begin{array}{l} ({\mu _0},{\Sigma _0}) = ({H_k}{{\hat x}_k},{H_k}{P_k}H_k^T)\\ ({\mu _1},{\Sigma _1}) = ({z_k},{R_k}) \end{array}$

融合两个高斯分布，得到

\begin{array}{l} H_{k} {\hat{x}}^{'}_{k} = H_{k} {\hat{x}}_{k} + K (z_{k} - H_{k} {\hat{x}}_{k}) \\ H_{k} {P^{'}}_{k} H_{k}^{T} = H_{k} P_{k} H_{k}^{T} - K H_{k} P_{k} H_{k}^{T} \end{array}

$\begin{array}{l} {H_k}{{\hat x'}_k} = {H_k}{{\hat x}_k} + K({z_k} - {H_k}{{\hat x}_k})\\ {H_k}{{P'}_k}H_k^T = {H_k}{P_k}H_k^T - K{H_k}{P_k}H_k^T \end{array}$
其中，卡尔曼增益为

K = H_{k} P_{k} H_{k}^{T} (H_{k} P_{k} H_{k}^{T} + R_{k})^{- 1}

$K = {H_k}{P_k}H_k^T{({H_k}{P_k}H_k^T + {R_k})^{ - 1}}$

得到最终的状态更新公式为

\begin{array}{l} {\hat{x}}^{'}_{k} = {\hat{x}}_{k} + K^{'} (z_{k} - H_{k} {\hat{x}}_{k}) \\ {P^{'}}_{k} = P_{k} - K^{'} H_{k} P_{k} \\ K^{'} = P_{k} H_{k}^{T} (H_{k} P_{k} H_{k}^{T} + R_{k})^{- 1} \end{array}

$\begin{array}{l} {{\hat x'}_k} = {{\hat x}_k} + K'({z_k} - {H_k}{{\hat x}_k})\\ {{P'}_k} = {P_k} - K'{H_k}{P_k}\\ K' = {P_k}H_k^T{({H_k}{P_k}H_k^T + {R_k})^{ - 1}} \end{array}$
上述公式中，

{\hat{x}}^{'}_{k}

${\hat x'}_k$ 就是新的最优估计，可以将其与

{\hat{P}}^{'}_{k}

${\hat P'}_k$ 放到下一个预测和更新方程中不断迭代。