5.13 卡尔曼滤波

卡尔曼滤波和递归最小二乘法形式上极其相似。递归最小二乘法最优解更新公式为
x^m=x^m−1+Kmϵmϵm=bm−armTx^m−1\mathbf{\hat{x}_{m}} =\mathbf{\hat{x}_{m-1}}+K_{m}\epsilon_{m}\\ \epsilon_{m} = b_{m}-\mathbf{a^T_{rm}}\mathbf{\hat{x}_{m-1}}\\ x^m=x^m−1+Kmϵmϵm=bm−armTx^m−1
其中 KmK_{m}Km 是新测量点的增益，预测残差 ϵm=bm−armTx^m−1\epsilon_{m} = b_{m}-\mathbf{a^T_{rm}}\mathbf{\hat{x}_{m-1}}ϵm=bm−armTx^m−1 为测量值 bmb_{m}bm 和预测值 armTx^m−1\mathbf{a^T_{rm}}\mathbf{\hat{x}_{m-1}}armTx^m−1 之差。

卡尔曼滤波形式上和上面公式一样，只是计算增益和残差的公式不同，但内涵不同。为了便于说明问题和对比，还是以跟踪车辆为例。假设车辆做匀加速直线运动 s=s0+v0t+1/2a0t2s = s_0 + v_0t + 1/2a_0t^2s=s0+v0t+1/2a0t2 ，我们需要获得加速度，可以测量不同时刻的位移 (ti,si)(t_i, s_i)(ti,si) ，即 tit_iti 时刻的位移为 sis_isi ，测量了 mmm 个数据。实际中为了简化系统，一般都是等间隔测量，即 ti+1−ti=ht_{i+1}-t_i=hti+1−ti=h 为常数。卡尔曼滤波和递归最小二乘法最大差别是增加了状态方程，这是本质差别！根据车辆做匀加速直线运动，我们可以建立车辆运动状态方程。车辆运动状态可以采用 tit_iti 时刻的位移、速度和加速度 (si,vi,ai)(s_i,v_i,a_i)(si,vi,ai) 表示， (si,vi,ai)(s_i,v_i,a_i)(si,vi,ai) 称为状态向量，记为 xi\mathbf{x}_ixi 。根据公式 s=s0+v0t+1/2a0t2s = s_0 + v_0t + 1/2a_0t^2s=s0+v0t+1/2a0t2 可以获得状态方程，
si=si−1+vi−1h+1/2ai−1h2vi=vi−1+ai−1hai=ai−1s_i = s_{i-1} + v_{i-1} h+ 1/2 a_{i-1} h^2 \\ v_i = v_{i-1} + a_{i-1} h \\ a_i = a_{i-1} \\ si=si−1+vi−1h+1/2ai−1h2vi=vi−1+ai−1hai=ai−1
因为状态向量为 xi=(si,vi,ai)\mathbf{x}_i = (s_i,v_i,a_i)xi=(si,vi,ai) ，所以
xi=Axi−1A=[1h1/2h201h001]\mathbf{x}_i = A \mathbf{x}_{i-1} \\ A = \left[ \begin{matrix} 1 &h &1/2h^2 \\ 0 &1&h \\ 0 &0&1 \end{matrix} \right] xi=Axi−1A=⎣⎡100h101/2h2h1⎦⎤
为状态转移方程，即根据 ti−1t_{i-1}ti−1 时刻的状态向量可以获得 tit_iti 时刻的状态向量。如果车辆是匀加速直线运动，则只需要知道初始时刻的状态向量 x0\mathbf{x}_0x0 就可以计算得到任意时刻的状态向量。这个性质理论上很完美，但实际中毫无用处。因为初始时刻的状态向量 x0\mathbf{x}_0x0 由于测量误差，会偏离理想值，后面任意时刻的状态向量的误差会随着时间增加而增加。例如根据公式 s=s0+v0t+1/2a0t2s = s_0 + v_0t + 1/2a_0t^2s=s0+v0t+1/2a0t2 得 s=s0′+δs0+(v0′+δv)t+1/2(a0+δa)t2=s0′+v0′t+1/2a0′t2+(δs0+δvt+1/2δat2)=s′+δss = s'_0+\delta_{s0} + (v'_0+\delta_v)t + 1/2(a_0+\delta_a)t^2=s'_0 + v'_0t + 1/2a'_0t^2 +(\delta_{s0} +\delta_vt+1/2\delta_at^2) = s' + \delta_ss=s0′+δs0+(v0′+δv)t+1/2(a0+δa)t2=s0′+v0′t+1/2a0′t2+(δs0+δvt+1/2δat2)=s′+δs ，其中 (s0′,v0′,a0′)(s'_0,v'_0,a'_0)(s0′,v0′,a0′) 是实际测量的初始状态向量，(δs0,δv,δa)(\delta_{s0},\delta_v,\delta_a)(δs0,δv,δa) 是偏差，则 s′s's′ 是实际获得的位移，也称为位移估计值，δs=δs0+δvt+1/2δat2\delta_s=\delta_{s0} +\delta_vt+1/2\delta_at^2δs=δs0+δvt+1/2δat2 是位移偏差，可见是时间的二次函数，随着时间增加，偏差增长速度很快。即使初始状态向量只有很小误差，当时间趋于无穷大时，偏差会趋于无穷大，导致位移估计值完全失效。所以根据状态转移方程只适合短期估计，不能用于长期估计。

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

si=si−1+vi−1h+1/2ai−1h2+δsvi=vi−1+ai−1h+δvai=ai−1+δas_i = s_{i-1} + v_{i-1} h+ 1/2 a_{i-1} h^2+\delta_s \\ v_i = v_{i-1} + a_{i-1} h+\delta_v \\ a_i = a_{i-1} +\delta_a \\ si=si−1+vi−1h+1/2ai−1h2+δsvi=vi−1+ai−1h+δvai=ai−1+δa

xi=Axi−1+δA=[1h1/2h201h001]δ=[δsδvδa]\mathbf{x}_i = A \mathbf{x}_{i-1} + \mathbf{\delta} \\ A = \left[ \begin{matrix} 1 &h &1/2h^2 \\ 0 &1&h \\ 0 &0&1 \end{matrix} \right] \mathbf{\delta} = \left[ \begin{matrix} \delta_s \\ \delta_v \\ \delta_a \end{matrix} \right] xi=Axi−1+δA=⎣⎡100h101/2h2h1⎦⎤δ=⎣⎡δsδvδa⎦⎤

向量 δ\mathbf{\delta}δ 是状态误差向量。

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

令 x^i\mathbf{\hat{x}}_ix^i 为 tit_iti 时刻状态向量的估计值，根据状态转移方程 xi=Axi−1+δ\mathbf{x}_i = A \mathbf{x}_{i-1} + \mathbf{\delta}xi=Axi−1+δ ，利用 ti−1t_{i-1}ti−1 时刻状态向量估计值 x^i−1\mathbf{\hat{x}}_{i-1}x^i−1 ，可得 tit_iti 时刻状态向量的预测值 x′i=Ax^i−1\mathbf{x'}_i = A\mathbf{\hat{x}}_{i-1}x′i=Ax^i−1 ，结合 tit_iti 时刻的测量值 sis_isi ，可得 tit_iti 时刻的状态向量估计值 x^i\mathbf{\hat{x}}_ix^i ，

x^i=x′i+Ki(si−si′)\mathbf{\hat{x}}_i = \mathbf{x'}_i + K_i(s_i - s'_i) x^i=x′i+Ki(si−si′)

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

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

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

系统测量方程为：
zi=Cxi+vi\mathbf{z}_i = C \mathbf{x}_{i} + \mathbf{v}_{i} zi=Cxi+vi
zi\mathbf{z}_izi 是测量向量，矩阵 CCC 是测量矩阵，vi\mathbf{v}_{i}vi 是测量噪声向量。假设测量噪声是零均值高斯白噪声，方差矩阵为 RRR 。比如车辆跟踪例子，测量值是车辆位移 sis_isi，状态向量是 si,vi,ais_i,v_i,a_isi,vi,ai ，则测量矩阵 C=[1,0,0]C = [1,0,0]C=[1,0,0] 。

则卡尔曼滤波方程为：

令 ti−1t_{i-1}ti−1 时刻状态向量估计值 x^i−1\mathbf{\hat{x}}_{i-1}x^i−1 ，可得 tit_iti 时刻状态向量的预测值 x′i=Ax^i−1\mathbf{x'}_i = A\mathbf{\hat{x}}_{i-1}x′i=Ax^i−1 ，结合 tit_iti 时刻的测量值 zi\mathbf{z}_izi ，可得 tit_iti 时刻的状态向量估计值 x^i\mathbf{\hat{x}}_ix^i ，

x′i=Ax^i−1x^i=x′i+Ki(zi−Cx′i)\mathbf{x'}_i = A\mathbf{\hat{x}}_{i-1} \\ \mathbf{\hat{x}}_i = \mathbf{x'}_i + K_i(\mathbf{z}_i - C\mathbf{x'}_i) x′i=Ax^i−1x^i=x′i+Ki(zi−Cx′i)

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

令 ti−1t_{i-1}ti−1 时刻状态向量估计值 x^i−1\mathbf{\hat{x}}_{i-1}x^i−1 的方差矩阵为 Pi−1P_{i-1}Pi−1 ，则根据状态转移方程 xi=Axi−1+Γwi−1\mathbf{x}_i = A \mathbf{x}_{i-1} + \mathbf{\Gamma} \mathbf{w}_{i-1}xi=Axi−1+Γwi−1 可得 tit_iti 时刻状态向量的预测值 x′i\mathbf{x'}_ix′i 的方差矩阵为 Pi′P'_iPi′ 为
Pi′=APi−1AT+ΓQΓTP'_i = AP_{i-1}A^T+\mathbf{\Gamma}Q\mathbf{\Gamma}^T Pi′=APi−1AT+ΓQΓT
卡尔曼增益矩阵 KiK_iKi 为
Ki=Pi′CT(CPi′CT+R)−1K_i = P'_iC^T(CP'_iC^T+R)^{-1} Ki=Pi′CT(CPi′CT+R)−1
其中 CPi′CTCP'_iC^TCPi′CT 是预测测量向量 Cx′iC\mathbf{x'}_iCx′i 的方差矩阵。根据该公式，把矩阵看成数量时，可以直观看出，当测量精度很高时，方差矩阵 RRR 元素值较小，矩阵也“较小”，所以卡尔曼增益矩阵“较大”，校正量更重要。当系统建模精度高时方差矩阵 QQQ 元素值较小，矩阵也“较小”，所以方差矩阵 Pi′P'_iPi′ 也“较小”，卡尔曼增益矩阵“较小”，预测量更重要。注意卡尔曼增益与测量值无关，独立于测量值，只与状态向量方差矩阵 PPP ，系统矩阵 AAA ，噪声转移矩阵 Γ\mathbf{\Gamma}Γ 和系统噪声方差矩阵 QQQ，测量矩阵 CCC 及测量噪声方差矩阵 RRR 有关。由于与测量值无关，故可以事先提前离线计算并保存，然后在线使用，这样就不需在线计算了。

tit_iti 时刻状态向量估计值 x^i\mathbf{\hat{x}}_ix^i 的方差矩阵 PiP_iPi 为
Pi=(E−KiC)Pi′P_i = (E-K_iC)P'_i Pi=(E−KiC)Pi′

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

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

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

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

推广到随机变量向量为(x1,x2,⋯,xn)(x_1,x_2,\cdots,x_n)(x1,x2,⋯,xn) ，随机变量向量的方差矩阵为
D=[dx1dx1x2⋯dx1xndx2x1dx2⋯dx2xn⋮dxnx1dxnx2⋯dxn]D = \left[ \begin{matrix} d_{x_1} & d_{x_1x_2} & \cdots & d_{x_1x_n}\\ d_{x_2x_1} & d_{x_2} & \cdots & d_{x_2x_n}\\ \vdots \\ d_{x_nx_1} & d_{x_nx_2} & \cdots & d_{x_n}\\ \end{matrix} \right] D=⎣⎢⎢⎢⎡dx1dx2x1⋮dxnx1dx1x2dx2dxnx2⋯⋯⋯dx1xndx2xndxn⎦⎥⎥⎥⎤
其中 dxid_{x_i}dxi 是分量 xix_ixi 的方差，dxixjd_{x_ix_j}dxixj 是分量 xi,xjx_i,x_jxi,xj 的协方差，根据协方差的定义，易得 dxixj=dxjxid_{x_ix_j}=d_{x_jx_i}dxixj=dxjxi ，所以方差矩阵是对称阵。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5.13 卡尔曼滤波相关推荐

2020保研夏令营——无科研无竞赛的夏令营之旅
个人情况: 学校:中国海洋大学(末9) 专业:计算机科学与技术前五学期平均学分绩:91.998 前五学期成绩排名:1/156 六级:467 获奖:只有学习奖学金其他:和物理系的同学参加过一项国创, ...
mahout安装测试
Mahout 是 Apache Software Foundation(ASF) 旗下的一个开源项目,提供一些可扩展的机器学习领域经典算法的实现,旨在帮助开发人员更加方便快捷地创建智能应用程序.Apa ...
卡尔曼滤波— Constant Velocity Model
假设你开车进入隧道,GPS信号丢失,现在我们要确定汽车在隧道内的位置.汽车的绝对速度可以通过车轮转速计算得到,汽车朝向可以通过yaw rate sensor(A yaw-rate sensor is ...
一文图解卡尔曼滤波(Kalman Filter)
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达译者注:这恐怕是全网有关卡尔曼滤波最简单易懂的解释,如果你认真的读 ...
解读基于多传感器融合的卡尔曼滤波算法
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|3D视觉工坊卡尔曼滤波器是传感器融合工程师用于自动驾驶 ...
卡尔曼滤波——基本假设（1）线性系统（2）高斯分布根据x(t) 求解x（t+1)
from:https://blog.csdn.net/u010720661/article/details/63253509 原文链接:http://www.bzarg.com/p/how-a-kal ...
三个最简单公式讲完卡尔曼滤波算法
概述首先明确一下卡尔曼滤波的基本概念:可参考知乎诸位大神的"如何通俗易懂地描述卡尔曼滤波",这里我也稍微说明一下. 所谓卡尔曼滤波就是当你在测量一个值时,同时拥有模型估计和直接测 ...
ad 卡尔曼_卡尔曼滤波算法C语言实现(转2)
本帖最后由 jackk 于 2014-3-5 13:00 编辑前段时间在论坛里简单地发了一些关于kalman的理解. 有很多网友顶贴的,趁着今天休息,整理一下前段时间的工作. 有些理解和说法可能不正 ...
卡尔曼滤波推导思路总结
推导思路一: (1) 混合高斯一维高斯函数形式: (1)N(x,μ,σ)=1σ2πe−(x−μ)22σ2\mathcal N(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2 ...

5.13 卡尔曼滤波

5.13 卡尔曼滤波

5.13 卡尔曼滤波相关推荐

最新文章

热门文章