5.13 卡尔曼滤波

5.13 卡尔曼滤波

卡尔曼滤波和递归最小二乘法形式上极其相似。递归最小二乘法最优解更新公式为
$$\begin{gathered} {\hat{\mathbf{x}}}_{\mathbf{m}}={\hat{\mathbf{x}}}_{\mathbf{m}-1}+K_m{ϵ}_m \\ {ϵ}_m=b_m-{\mathbf{a}}_{\mathbf{r}\mathbf{m}}^{\mathbf{T}}{\hat{\mathbf{x}}}_{\mathbf{m}-1} \end{gathered}$$
其中 $K_m$ 是新测量点的增益，预测残差 ${ϵ}_m=b_m-{\mathbf{a}}_{\mathbf{r}\mathbf{m}}^{\mathbf{T}}{\hat{\mathbf{x}}}_{\mathbf{m}-1}$ 为测量值 $b_m$ 和预测值 ${\mathbf{a}}_{\mathbf{r}\mathbf{m}}^{\mathbf{T}}{\hat{\mathbf{x}}}_{\mathbf{m}-1}$ 之差。

卡尔曼滤波形式上和上面公式一样，只是计算增益和残差的公式不同，但内涵不同。为了便于说明问题和对比，还是以跟踪车辆为例。假设车辆做匀加速直线运动 $s=s_0+v_{0}t+1/2a_0{t}^2$ ，我们需要获得加速度，可以测量不同时刻的位移 $(t_i,s_i)$ ，即 $t_i$ 时刻的位移为 $s_i$ ，测量了 $m$ 个数据。实际中为了简化系统，一般都是等间隔测量，即 $t_{i+1}-t_i=h$ 为常数。卡尔曼滤波和递归最小二乘法最大差别是增加了状态方程，这是本质差别！根据车辆做匀加速直线运动，我们可以建立车辆运动状态方程。车辆运动状态可以采用 $t_i$ 时刻的位移、速度和加速度 $(s_i,v_i,a_i)$ 表示， $(s_i,v_i,a_i)$ 称为状态向量，记为 ${\mathbf{x}}_i$ 。根据公式 $s=s_0+v_{0}t+1/2a_0{t}^2$ 可以获得状态方程，
$$\begin{gathered} s_i=s_{i-1}+v_{i-1}h+1/2a_{i-1}{h}^2 \\ {v}_i=v_{i-1}+a_{i-1}h \\ {a}_i=a_{i-1} \end{gathered}$$
因为状态向量为 ${\mathbf{x}}_i=(s_i,v_i,a_i)$ ，所以
$$\begin{gathered} {\mathbf{x}}_i=A{\mathbf{x}}_{i-1} \\ A=\left[\begin{array}{ccc}1& h& 1/2h^2\\ 0& 1& h\\ 0& 0& 1\end{array}\right] \end{gathered}$$
为状态转移方程，即根据 $t_{i-1}$ 时刻的状态向量可以获得 $t_i$ 时刻的状态向量。如果车辆是匀加速直线运动，则只需要知道初始时刻的状态向量 ${\mathbf{x}}_0$ 就可以计算得到任意时刻的状态向量。这个性质理论上很完美，但实际中毫无用处。因为初始时刻的状态向量 ${\mathbf{x}}_0$ 由于测量误差，会偏离理想值，后面任意时刻的状态向量的误差会随着时间增加而增加。例如根据公式 $s=s_0+v_{0}t+1/2a_0{t}^2$ 得 $s=s_0^{\prime }+{\delta }_{s0}+(v_0^{\prime }+{\delta }_v)t+1/2(a_0+{\delta }_a)t^2=s_0^{\prime }+v_0^{\prime }t+1/2a_0^{\prime }{t}^2+({\delta }_{s0}+{\delta }_{v}t+1/2{\delta }_a{t}^2)=s^{\prime }+{\delta }_s$ ，其中 $(s_0^{\prime },v_0^{\prime },a_0^{\prime })$ 是实际测量的初始状态向量，$({\delta }_{s0},{\delta }_v,{\delta }_a)$ 是偏差，则 $s^{\prime }$ 是实际获得的位移，也称为位移估计值，${\delta }_s={\delta }_{s0}+{\delta }_{v}t+1/2{\delta }_a{t}^2$ 是位移偏差，可见是时间的二次函数，随着时间增加，偏差增长速度很快。即使初始状态向量只有很小误差，当时间趋于无穷大时，偏差会趋于无穷大，导致位移估计值完全失效。所以根据状态转移方程只适合短期估计，不能用于长期估计。

状态转移方程是假设$t_{i-1}$ 时刻到 $t_i$ 时刻这段时间内，车辆是匀加速直线运动，但实际中，车辆不可能是严格的匀加速，加速度会存在波动。所以状态转移方程严格为

$$\begin{gathered} s_i=s_{i-1}+v_{i-1}h+1/2a_{i-1}{h}^2+{\delta }_s \\ {v}_i=v_{i-1}+a_{i-1}h+{\delta }_v \\ {a}_i=a_{i-1}+{\delta }_a \end{gathered}$$

$$\begin{gathered} {\mathbf{x}}_i=A{\mathbf{x}}_{i-1}+\delta \\ A=\left[\begin{array}{ccc}1& h& 1/2h^2\\ 0& 1& h\\ 0& 0& 1\end{array}\right]\delta =\left[\begin{array}{c}{\delta }_s\\ {\delta }_v\\ {\delta }_a\end{array}\right] \end{gathered}$$

向量 $\delta$ 是状态误差向量。

为了准确获得状态向量，需要测量，假设只能测量位移值 $s_i$ 。

令 ${\hat{\mathbf{x}}}_i$ 为 $t_i$ 时刻状态向量的估计值， 根据状态转移方程 ${\mathbf{x}}_i=A{\mathbf{x}}_{i-1}+\delta$ ，利用 $t_{i-1}$ 时刻状态向量估计值 ${\hat{\mathbf{x}}}_{i-1}$ ，可得 $t_i$ 时刻状态向量的预测值 ${{\mathbf{x}}^{\prime }}_i=A{\hat{\mathbf{x}}}_{i-1}$ ，结合 $t_i$ 时刻的测量值 $s_i$ ，可得 $t_i$ 时刻的状态向量估计值 ${\hat{\mathbf{x}}}_i$ ，

$${\hat{\mathbf{x}}}_i={{\mathbf{x}}^{\prime }}_i+K_i(s_i-s_i^{\prime })$$

其中 $K_i$ 称为卡尔曼增益，是矩阵。$s_i^{\prime }$ 是预测值 ${{\mathbf{x}}^{\prime }}_i$ 的位移分量，所以 $s_i-s_i^{\prime }$ 就是实际测量值与预测测量值的差，类似残差。卡尔曼的核心是如何计算增益 $K_i$ 使估计值 ${\hat{\mathbf{x}}}_i$ 最优。本节不打算推导计算过程。

如何理解公式 ${\hat{\mathbf{x}}}_i={{\mathbf{x}}^{\prime }}_i+K_i(s_i-s_i^{\prime })$ ？计算 ${{\mathbf{x}}^{\prime }}_i$ 是利用状态转移方程，状态转移方程可以看作是对系统进行理论建模获得的理论模型，这个预测值可以看作是理论结果，由于建模有误差和初始状态有偏差，导致该理论结果长期不可靠，故需要实时测量值进行校正。$s_i-s_i^{\prime }$ 就是实际测量值与预测测量值的差，利用测量值和理论值的偏差进行校正，校正系数就是卡尔曼增益，以获得最优估计值。最优估计值就是平衡理论预测值 ${{\mathbf{x}}^{\prime }}_i$ 和校正量 $(s_i-s_i^{\prime })$ 的比重达到最优。其总的原则是，如果测量精度很高，即应该更相信校正量，故校正量比重增大，卡尔曼增益“变大”；如果系统建模精度很高，系统很稳定，完全掌握了系统变化规律，则理论预测值比重增大，卡尔曼增益“变小”。

把上述过程进行一般化，得到卡尔曼滤波。系统状态转移方程为：
$${\mathbf{x}}_i=A{\mathbf{x}}_{i-1}+\mathbf{\Gamma }{\mathbf{w}}_{i-1}$$
${\mathbf{x}}_i$ 是状态向量，矩阵 $A$ 是状态转移矩阵，${\mathbf{w}}_{i-1}$ 是系统噪声向量，$\mathbf{\Gamma }$ 是系统噪声转移矩阵。假设系统噪声是零均值高斯白噪声，方差矩阵为 $Q$ 。

系统测量方程为：
$${\mathbf{z}}_i=C{\mathbf{x}}_i+{\mathbf{v}}_i$$
${\mathbf{z}}_i$ 是测量向量，矩阵 $C$ 是测量矩阵，${\mathbf{v}}_i$ 是测量噪声向量。假设测量噪声是零均值高斯白噪声，方差矩阵为 $R$ 。比如车辆跟踪例子，测量值是车辆位移 $s_i$，状态向量是 $s_i,v_i,a_i$ ，则测量矩阵 $C=[1,0,0]$ 。

则卡尔曼滤波方程为：

令 $t_{i-1}$ 时刻状态向量估计值 ${\hat{\mathbf{x}}}_{i-1}$ ，可得 $t_i$ 时刻状态向量的预测值 ${{\mathbf{x}}^{\prime }}_i=A{\hat{\mathbf{x}}}_{i-1}$ ，结合 $t_i$ 时刻的测量值 ${\mathbf{z}}_i$ ，可得 $t_i$ 时刻的状态向量估计值 ${\hat{\mathbf{x}}}_i$ ，

$$\begin{gathered} {{\mathbf{x}}^{\prime }}_i=A{\hat{\mathbf{x}}}_{i-1} \\ {\hat{\mathbf{x}}}_i={{\mathbf{x}}^{\prime }}_i+K_i({\mathbf{z}}_i-C{{\mathbf{x}}^{\prime }}_i) \end{gathered}$$

$K_i$ 称为卡尔曼增益矩阵，其可递归计算。$C{{\mathbf{x}}^{\prime }}_i$ 是预测值 ${{\mathbf{x}}^{\prime }}_i$ 对应的预测测量向量，${\mathbf{z}}_i-C{{\mathbf{x}}^{\prime }}_i$ 是真实测量向量与预测测量向量之差，可以认为是新测量所获得的信息，称为新息。如果新息为零，说明测量向量和预测向量相等，测量向量没有带来关于系统的任何新信息。

令 $t_{i-1}$ 时刻状态向量估计值 ${\hat{\mathbf{x}}}_{i-1}$ 的方差矩阵为 $P_{i-1}$ ，则根据状态转移方程 ${\mathbf{x}}_i=A{\mathbf{x}}_{i-1}+\mathbf{\Gamma }{\mathbf{w}}_{i-1}$ 可得 $t_i$ 时刻状态向量的预测值 ${{\mathbf{x}}^{\prime }}_i$ 的方差矩阵为 $P_i^{\prime }$ 为
$$P_i^{\prime }=A{P}_{i-1}{A}^T+\mathbf{\Gamma }Q{\mathbf{\Gamma }}^T$$
卡尔曼增益矩阵 $K_i$ 为
$$K_i=P_i^{\prime }{C}^T(C{P}_i^{\prime }{C}^T+R{)}^{-1}$$
其中 $C{P}_i^{\prime }{C}^T$ 是预测测量向量 $C{{\mathbf{x}}^{\prime }}_i$ 的方差矩阵。根据该公式，把矩阵看成数量时，可以直观看出，当测量精度很高时，方差矩阵 $R$ 元素值较小，矩阵也“较小”，所以卡尔曼增益矩阵“较大”，校正量更重要。当系统建模精度高时方差矩阵 $Q$ 元素值较小，矩阵也“较小”，所以方差矩阵 $P_i^{\prime }$ 也“较小”，卡尔曼增益矩阵“较小”，预测量更重要。注意卡尔曼增益与测量值无关，独立于测量值，只与状态向量方差矩阵 $P$ ，系统矩阵 $A$ ，噪声转移矩阵 $\mathbf{\Gamma }$ 和系统噪声方差矩阵 $Q$，测量矩阵 $C$ 及测量噪声方差矩阵 $R$ 有关。由于与测量值无关，故可以事先提前离线计算并保存，然后在线使用，这样就不需在线计算了。

$t_i$ 时刻状态向量估计值 ${\hat{\mathbf{x}}}_i$ 的方差矩阵 $P_i$ 为
$$P_i=(E-K_{i}C)P_i^{\prime }$$

方差矩阵 $P_i$ 具有明显的物理意义，其对角元素值为状态向量对应分量的方差，表明了估计值精度。

实际应用时，只需要知道初始状态向量估计值 ${\hat{\mathbf{x}}}_0$ 和初始方差矩阵 $P_0$ ，即可递推计算后面时刻的状态向量估计值。

理解卡尔曼滤波难点在于方差矩阵，这也是实际应用中的难点。对于单个变量的方差，读者应该比较熟悉。方差是衡量随机变量变动范围的一个数值指标，方差越大说明随机变量变动范围越大，其计算公式为
$$d_x=\sum (x_i-\bar{x}{)}^2/(n-1)$$
其中 $x_i$ 是随机变量的采样值，$n$ 是采样次数。

对于随机变量向量的方差矩阵，怎么理解呢？假设随机变量向量只有两个分量为$(x,y)$ ，分量 $x$ 的方差为 $d_x$ ，分量 $y$ 的方差为 $d_y$ 。如果这两个分量线性相关，则协方差不为零，协方差定义为
$$d_{xy}=\sum [(x_i-\bar{x})(y_i-\bar{y})]/(n-1)$$
此时随机变量向量的方差矩阵为
$$D=\left[\begin{array}{cc}{d}_x& d_{xy}\\ d_{xy}& d_y\end{array}\right]$$

推广到随机变量向量为$(x_1,x_2,\cdots ,x_n)$ ，随机变量向量的方差矩阵为
$$D=\left[\begin{array}{cccc}{d}_{x_1}& d_{x_1{x}_2}& \cdots & d_{x_1{x}_n}\\ d_{x_2{x}_1}& d_{x_2}& \cdots & d_{x_2{x}_n}\\ ⋮& & & \\ d_{x_n{x}_1}& d_{x_n{x}_2}& \cdots & d_{x_n}\end{array}\right]$$
其中 $d_{x_i}$ 是分量 $x_i$ 的方差，$d_{x_i{x}_j}$ 是分量 $x_i,x_j$ 的协方差，根据协方差的定义，易得 $d_{x_i{x}_j}=d_{x_j{x}_i}$ ，所以方差矩阵是对称阵。

实际应用时，要根据系统的性质，首先选择合适的状态向量来表征系统，然后对系统建模，获得状态转移方程，难点是获得系统噪声方差矩阵。测量方程一般容易建模，测量噪声方差矩阵也比较容易获得，传感器的方差就是对应分量的方差，如果测量向量分量不相关，则测量噪声方差矩阵就是对角阵。

卡尔曼滤波具有很多优点，计算是递归的，计算很高效，且不需要保存历史数据。结合状态转移方程和测量方程，最优估计值的精度比测量值的精度可以提高一个数量级，也就是说,可用低精度的传感器来构成高精度的测量系统，只需增加低成本的计算机进行滤波。估计值精度提高了，但为此付出很大代价，因为我们需要额外对系统进行建模获得状态转移方程，需要知道系统噪声方差矩阵和测量方差矩阵，这需要领域专业知识，对领域越精通，建模越精确，则估计值精度越高。如果对领域一无所知，则估计值精度就是传感器精度，这个就类似最小二乘法，不需要知道系统任何知识。这体现了知识的价值，卡尔曼滤波完美地实现了知识的价值。在机器学习，领域知识也称为先验知识，利用先验知识也能提高系统性能。为什么称为滤波呢？因为估计值精度比测量值精度高，这样就相当于去除了测量值的噪声，去除噪声就是滤波。

虽然卡尔曼滤波器有很大优点，但是，在理论上它需要精确的系统模型即状态转移方程和系统方差矩阵，这在实际中是很难精确获得的。如果不能克服这些困难，将会造成滤波器估计误差增大，甚至发散。

还是以前面的车辆运动状态估计为例。如果默认车辆做匀加速运动，则模型噪声方差 $Q$ 很小，因为加速度基本不变。此时卡尔曼增益很小，校正量的权重小，测量值不起作用，估计值主要依靠系统模型推算。但如果车辆突然进行机动运动即突然加速或减速，模型噪声方差 $Q$ 变大，则匀加速运动假设不成立，系统模型出现大的偏差，此时估计值还主要依靠系统模型推算的话，会出现很大偏差，此时需要增加测量值的作用。所以一个实际系统，系统模型即状态转移方程和系统方差矩阵会发生变化，并不是一直稳定的，特别是系统方差矩阵。所以卡尔曼滤波必须自适应系统模型的变化，以达到最佳，即模型噪声方差 $Q$ 很小时，减小卡尔曼增益，否则增加卡尔曼增益。这里面有个矛盾，如果卡尔曼增益增加很多的话，则预测值比重降低，主要依靠测量值进行估计，则估计值精度和测量值精度差不多，达不到滤波以提高精度的效果。如果卡尔曼增益很小的话，主要依靠预测值进行估计，精度是提高了很多，但有可能导致跟踪失败，不能跟随目标的机动运动。总之，如果更相信测量值，则能快速跟随系统，但精度提高有限；如果更相信系统模型，则精度能提高很多，但可能会导致跟踪失败，需要衡量这两个方面。这也给反跟踪提供了思路，即做机动运动，增加雷达跟踪的难度，这和生活常识一致。飞机要摆脱雷达跟踪，必须时而加速，时而减速，时而转弯。

实际中怎么知道系统的模型噪声方差发生变化呢？主要根据新息 ${\mathbf{z}}_i-C{{\mathbf{x}}^{\prime }}_i$ 。如果新息出现异常值，一般主要有两种可能，一种是测量值 ${\mathbf{z}}_i$ 出现异常，另一种是系统发生机动运动导致预测值 ${{\mathbf{x}}^{\prime }}_i$ 远离实际值。测量值一般满足高斯分布，偶尔出现异常值有可能，但连续出现异常值的概率很低，此时就可认为是系统发生机动运动，模型噪声方差变大。故此时应增加系统噪声方差，以提高卡尔曼增益，测量值权重增加，来快速跟随系统。系统机动运动后又稳定下来，此时新息变小，没有大的异常值，故此时应减小系统噪声方差，以减小卡尔曼增益，预测值权重增加，来提高精度。

判断新息是否出现异常，是否又稳定了，要对新息进行统计。当模型与实际系统完全匹配时，新息是零均值的高斯分布。系统正常情况下对新息进行统计，获得每个分量的标准差。对新息的任一分量，如果连续多个时刻，绝对值较大则可以认为出现异常，模型噪声方差变大，此时应增加系统噪声方差。如果连续多个时刻，绝对值较小则可认为系统恢复稳定，模型噪声方差恢复为以前的值。如何定量衡量新息变大的程度呢？首先把新息分量标准化即除以对应标准差，然后计算连续多个时刻的标准化后新息分量的平方和，如果平方和较大，则判断出现异常。平方和分布是三大分布之一即卡方分布 ${\chi }_n^2$。若 $X\sim {\chi }_n^2$, 记 $P(x>c)=\alpha$，则 $c={\chi }_n^2(\alpha )$ 称为 ${\chi }_n^2$ 分布的上侧 $\alpha$ 分位数。当 $\alpha$ 和 $n$ 给定时可查表求出 ${\chi }_n^2(a)$ 之值，如 ${\chi }_{10}^2(0.01)=23.209$，${\chi }_5^2(0.05)=12.592$ 等。${\chi }_5^2(0.05)=12.592$ 表示连续 $5$ 个标准化后新息分量的平方和大于 $12.592$ 概率为 $5\%$ 。具体取连续多少 $n$ 个平方和及 $\alpha$ 分位数，需要平衡快速性和抗噪性。如果设置得很敏感，即 $n$ 较小如 $2$ 个，$\alpha$ 较大如 $20\%$，则一旦新息有点异常就变大模型噪声方差，则系统响应快速，但不抗噪，因为有可能是传感器噪声引起的异常，系统也能快速跟随测量噪声。反之，即 $n$ 较大，$\alpha$ 较小，即使新息有异常也需过很久才变大模型噪声方差，则系统响应速度很慢，但很抗噪。

如果新息一直很大，很有可能是测量噪声方差设置得比实际值偏小，需要加大测量噪声方差。反之，如果新息一直很小，很有可能是测量噪声方差设置得比实际值偏大，需要适度减小测量噪声方差。

在理论上，随着测量数据的增多,便能得到更精确的估计值。但是，在实际中并非这样简单。有时，随着滤波时间增长，估计值实际方差反而不断增大，趋于无穷。这称为滤波发散。发散原因主要是模型存在误差。例如对物理间题了解不够精确，或人为的模型简化，对系统噪声模型了解不够正确等等。模型误差引起发散的情况，往往是随着滤波时间增长，状态向量的方差矩阵越来越小，引
起卡尔曼增益变小，使测量值越来越不起作用，滤波器越来越依赖于不正确的模型，最后引起发散。为了克服模型误差引起的发散，一种途径是用限定下界法，伪噪声法等各种方法，不使卡尔曼增益过分小。限定下界法对状态向量的方差矩阵规定一个下限，或直接对卡尔曼增益规定一个下限。可是，这种估计是有偏的。伪噪声法控制是人为加大系统噪声方差矩阵 $Q$ ，用扩大的机动噪声代替模型误差。$Q$ 加大了卡尔曼增益也必然增大。另一种途径是增加新测量值的权重，具体方法有衰减记忆，限定记忆和直接加权等。因为卡尔曼滤波采用了所有的历史数据来进行估计，就如同时间衰减递归最小二乘法，我们可以减小历史数据的权重，增加当前时刻测量数据的权重，达到增加卡尔曼增益。

系统噪声方差矩阵 $Q$ 和测量方差矩阵 $R$ ，一般来说很难获得精确值，为了避免滤波发散，在可能的取值范围内应该选择较大的值。状态向量初始方差矩阵 $P_0$ 也应该选择较大的值。

滤波发散另一原因是计算舍入误差导致，因为计算卡尔曼增益需要计算逆矩阵，很容易导致数值不稳定。解决方法是对方差矩阵 $P_i$ 进行平方根分解，提高数值稳定性。

当状态转移方程、测量方程中有关矩阵都与时刻无关，如 $A,C,\Gamma ,Q,R$ ，称为时不变系统。在此假设下，卡尔曼增益矩阵 $K_i$ 当时间区域无穷大即 $i\to \infty$ 时存在极限，令极限为$K$ 。递推计算过程中用 $K$ 代替所有的 $K_i$ ，可极大提高计算效率和减小存储零。$K_i$ 是指数收敛的，故收敛速度很快。近似代替导致的估计误差也是指数收敛于零，所以估计误差可忽略不计。理论上计算 $K$ 需要解矩阵的Riccati方程，十分困难，实际中可计算机仿真得到，即多次递归计算 $K_i$ ，取极限为 $K$ 。

极限卡尔曼增益矩阵 $K$ 只与矩阵 $A,C,\Gamma ,Q,R$ 有关，与初始状态向量的方差矩阵 $P_0$ 无关。此时状态向量估计值更新方程为 ${\hat{\mathbf{x}}}_i={{\mathbf{x}}^{\prime }}_i+K({\mathbf{z}}_i-C{{\mathbf{x}}^{\prime }}_i)$ 。

如果状态转移方程、测量方程中有关矩阵与时刻相关，如 $A,C,\Gamma ,Q,R$ ，称为时变系统。注意只要有一个与时刻相关，都可称为时变系统。时变系统即系统随时间会变化，比如系统噪声 $Q$ 会发生改变，传感器性能 $R$ 会老化，甚至系统本身 $A$ 会缓慢变化。时变系统的滤波公式也如前面所述，只是 $A,C,\Gamma ,Q,R$ 是时刻不同的。

卡尔曼滤波假设噪声都是服从高斯分布的，如果不服从高斯分布，则需对滤波系统进行改进。如有色噪声，可以把有色噪声变换成高斯噪声，从而可适应卡尔曼滤波。具体内容请查阅相关资料。

卡尔曼滤波中状态转移方程是线性的，即是线性系统。但实际中存在大量非线性系统，则可以采用高数中化曲为直的思想，利用泰勒展开式把系统模型变成线性，从而采用卡尔曼滤波，这称为扩展卡尔曼滤波。具体内容请查阅相关资料。