卡尔曼滤波 (英语:Kalman filter )是一种高效率的递归滤波器 (自回归 滤波器),它能够从一系列的不完全及包含噪声 的测量 中,估计动态系统 的状态。卡尔曼滤波会根据各测量量在不同时间下的值,考虑各时间下的联合分布 ,再产生对未知变数的估计,因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼 。
卡尔曼滤波器会追踪系统的估计状态,以及估计的变异量或是不确定性。会透过状状态转换模型以及其量测来更新估计值。x ^ k ∣ k − 1 {\displaystyle {\hat {x}}_{k\mid k-1}} 是指时间k 时的估计,还没有考虑第k 次量测y k 的资讯,P k ∣ k − 1 {\displaystyle P_{k\mid k-1}} 为对应的不确定性 卡尔曼滤波在技术领域有许多的应用。常见的有飞机及太空船的导引、导航及控制 [1] 。卡尔曼滤波也广为使用在时间序列 的分析中,例如信号处理 及计量经济学 中。卡尔曼滤波也是机器人 运动规划及控制的重要主题之一,有时也包括在轨迹最佳化 。卡尔曼滤波也用在中轴神经系统运动控制的建模中。因为从给与运动命令到收到感觉神经的回授之间有时间差,使用卡尔曼滤波有助于建立符合实际的系统,估计运动系统的目前状态,并且更新命令[2] 。
卡尔曼滤波的算法是二步骤的程序。在估计步骤中,卡尔曼滤波会产生有关目前状态的估计,其中也包括不确定性。只要观察到下一个量测(其中一定含有某种程度的误差,包括随机噪声)。会通过加权平均 来更新估计值,而确定性越高的量测加权比重也越高。算法是迭代的,可以在实时控制系统 中执行,只需要目前的输入量测、以往的计算值以及其不确定性矩阵,不需要其他以往的资讯。
使用卡尔曼滤波不用假设误差是正态分布 [3] ,不过若所有的误差都是正态分布,卡尔曼滤波可以得到正确的条件几率估计。
也发展了一些扩展或是广义的卡尔曼滤波,例如运作在非线性系统的扩展卡尔曼滤波 及无迹卡尔曼滤波(英语:unscented Kalman filter )。底层的模型类似隐马尔可夫模型 ,不过潜在变量 的状态空间是连续的,而且所有潜在变量及可观测变数都是正态分布。
应用实例
编辑
卡尔曼滤波的一个典型实例是从一组有限的,包含噪声的,通过对物体位置的观察序列(可能有偏差)预测出物体的位置的坐标 及速度 。在很多工程应用(如雷达 、机器视觉 )中都可以找到它的身影。同时,卡尔曼滤波也是控制理论 以及控制系统工程 中的一个重要课题。
例如,对于雷达来说,人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响,得到一个关于目标位置的好的估计 。这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测),也可以是对过去位置的估计(插值或平滑)。
这种滤波方法以它的发明者鲁道夫·卡尔曼 (Rudolph Kalman)命名,但是根据文献可知实际上Peter Swerling在更早之前就提出了一种类似的算法。
斯坦利·施密特 (Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA艾姆斯研究中心 访问时,发现他的方法对于解决阿波罗计划 的轨道预测很有用,后来阿波罗飞船的导航电脑便使用了这种滤波器。关于这种滤波器的论文由Swerling(1958)、Kalman (1960)与Kalman and Bucy(1961)发表。
目前,卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单 卡尔曼滤波器。除此以外,还有施密特扩展 滤波器、信息 滤波器以及很多Bierman, Thornton开发的平方根 滤波器的变种。也许最常见的卡尔曼滤波器是锁相环 ,它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。
以下的讨论需要线性代数 以及概率论 的一般知识。
演算概念
编辑
卡尔曼滤波器使用系统的动态模型(例如,运动的物理定律),该系统的已知控制输入以及多个顺序的测量值(例如来自传感器的测量值)来形成对系统变化量(其状态)更好的估计,其精度比仅使用一种测量获得的估算值高。它是一种常见的感测器融合 和数据融合 算法。
感测器数据的噪声,描述系统演化的方程式的近似值以及未考虑所有因素的外部因素都限制了确定系统状态的能力。卡尔曼滤波器有效地处理了由于感测器数据噪声引起的不确定性,并在一定程度上处理了随机外部因素。卡尔曼滤波器使用加权平均值 生成系统状态的估计值,作为系统预测状态和新测量值的平均值。权重的目的是估计值具有更好(即较小)的不确定性的值会被更多“信任”。权重是根据共变异数 来计算的,共变异数是对系统状态预测的估计不确定性的度量。加权平均值的结果是介于预测状态和测量状态之间的新状态估计,并且比任何一个状态都有更好的估计不确定性。在每个时间步重复此过程,新的估计值及其共变异数将通知后续迭代中使用的预测。这意味着卡尔曼滤波器可以递回 地工作,并且只需要系统状态的最后“最佳猜测”,而不是整个历史,就可以计算新状态。
测量和当前状态估计的相对确定性是重要的考虑因素,通常根据卡尔曼滤波器的增益 来讨论滤波器的反应。卡尔曼增益是赋予测量值和当前状态估计值的相对权重,可以进行“调整”以获得特定的性能。增益高时,滤波器将更多的精力放在最新的测量上,因此反应速度更快。增益较低时,滤波器会更紧密地遵循模型预测。在极端情况下,接近1的高增益将导致估计的轨迹更加跳跃,而接近零的低增益将消除噪声,但会降低反应速度。
在执行滤波器的实际计算时(如下所述),状态估计值和共变异数被编码到矩阵 中,以处理单个计算集中涉及的多个维度。这允许在任何过渡模型或共变异数中表示不同状态变量(例如位置,速度和加速度)之间的线性关系。
基本动态系统模型
编辑
卡尔曼滤波建立在线性代数 和隐马尔可夫模型 (hidden Markov model)上。其基本动态系统可以用一个马尔可夫链 表示,该马尔可夫链建立在一个被高斯噪声 (即正态分布的噪声)干扰的线性算子 上的。系统的状态 可以用一个元素为实数的向量 表示。随着离散时间 的每一个增加,这个线性算子 就会作用在当前状态 上,产生一个新的状态,并也会带入一些噪声,同时系统的一些已知的控制器的控制信息也会被加入。同时,另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。
为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态,必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k ,定义矩阵 F k , H k , Q k , R k ,有时也需要定义B k ,如下。
卡尔曼滤波器的模型。圆圈代表向量 ,方块代表矩阵 ,星号代表高斯噪声 ,其协方差矩阵 在右下方标出。 卡尔曼滤波模型假设k 时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式:
x k = F k x k − 1 + B k u k + w k {\displaystyle {\textbf {x}}_{k}={\textbf {F}}_{k}{\textbf {x}}_{k-1}+{\textbf {B}}_{k}{\textbf {u}}_{k}+{\textbf {w}}_{k}} 其中
F k 是作用在x k −1 上的状态变换模型(/矩阵/向量)。
B k 是作用在控制器向量u k 上的输入-控制模型。
w k 是过程噪声,并假定其符合均值为零,协方差矩阵 为Q k 的多元正态分布 。w k ∼ N ( 0 , Q k ) {\displaystyle {\textbf {w}}_{k}\sim N(0,{\textbf {Q}}_{k})} 时刻k ,对真实状态x k 的一个测量z k 满足下式:
z k = H k x k + v k {\displaystyle {\textbf {z}}_{k}={\textbf {H}}_{k}{\textbf {x}}_{k}+{\textbf {v}}_{k}} 其中H k 是观测模型,它把真实状态空间映射成观测空间,v k 是观测噪声,其均值为零,协方差矩阵为R k ,且服从正态分布 。
v k ∼ N ( 0 , R k ) {\displaystyle {\textbf {v}}_{k}\sim N(0,{\textbf {R}}_{k})} 初始状态以及每一时刻的噪声{x 0 , w 1 , ..., w k , v 1 ... v k }都认为是互相独立 的。
实际上,很多真实世界的动态系统都并不确切的符合这个模型;但是由于卡尔曼滤波器被设计在有噪声的情况下工作,一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种,在下边讨论中有描述。
卡尔曼滤波器
编辑
考虑在无摩擦的、无限长的直轨道上的一辆车。该车最初停在位置0处,但时不时受到随机的冲击。每隔Δt 秒即测量车的位置,但是这个测量是非精确的;想建立一个关于其位置以及速度 的模型。来看如何推导出这个模型以及如何从这个模型得到卡尔曼滤波器。
因为车上无动力,所以可以忽略掉B k 和u k 。由于F 、H 、R 和Q 是常数,所以时间下标可以去掉。
车的位置以及速度(或者更加一般的,一个粒子的运动状态)可以被线性状态空间描述如下:
x k = [ x x ˙ ] {\displaystyle {\textbf {x}}_{k}={\begin{bmatrix}x\\{\dot {x}}\end{bmatrix}}} 其中x ˙ {\displaystyle {\dot {x}}} 是速度,也就是位置对于时间的导数。
假设在(k − 1)时刻与k 时刻之间,车受到a k 的加速度,其符合均值为0,标准差为σ a 的正态分布 。根据牛顿运动定律 ,可以推出
x k = F x k − 1 + G a k {\displaystyle {\textbf {x}}_{k}={\textbf {F}}{\textbf {x}}_{k-1}+{\textbf {G}}a_{k}} 其中
F = [ 1 Δ t 0 1 ] {\displaystyle {\textbf {F}}={\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}}} 且
G = [ Δ t 2 2 Δ t ] {\displaystyle {\textbf {G}}={\begin{bmatrix}{\frac {\Delta t^{2}}{2}}\\\Delta t\end{bmatrix}}} 可以发现
Q = cov ( G a ) = E [ ( G a ) ( G a ) T ] = G E [ a 2 ] G T = G [ σ a 2 ] G T = σ a 2 G G T {\displaystyle {\textbf {Q}}={\textrm {cov}}({\textbf {G}}a)={\textrm {E}}[({\textbf {G}}a)({\textbf {G}}a)^{T}]={\textbf {G}}{\textrm {E}}[a^{2}]{\textbf {G}}^{T}={\textbf {G}}[\sigma _{a}^{2}]{\textbf {G}}^{T}=\sigma _{a}^{2}{\textbf {G}}{\textbf {G}}^{T}} (因为σ a 是一个标量)。在每一时刻,对其位置进行测量,测量受到噪声干扰。假设噪声服从正态分布,均值为0,标准差为σ z 。
z k = Hx k + v k {\displaystyle {\textbf {z}}_{k}={\textbf {Hx}}_{k}+{\textbf {v}}_{k}} 其中
H = [ 1 0 ] {\displaystyle {\textbf {H}}={\begin{bmatrix}1&0\end{bmatrix}}} 且
R = E [ v k v k T ] = [ σ z 2 ] {\displaystyle {\textbf {R}}={\textrm {E}}[{\textbf {v}}_{k}{\textbf {v}}_{k}^{T}]={\begin{bmatrix}\sigma _{z}^{2}\end{bmatrix}}} 如果知道足够精确的车最初的位置,那么可以初始化
x ^ 0 | 0 = [ 0 0 ] {\displaystyle {\hat {\textbf {x}}}_{0|0}={\begin{bmatrix}0\\0\end{bmatrix}}} 并且,若让滤波器知道确切的初始位置,可给出一个协方差矩阵:
P 0 | 0 = [ 0 0 0 0 ] {\displaystyle {\textbf {P}}_{0|0}={\begin{bmatrix}0&0\\0&0\end{bmatrix}}} 如果不确切的知道最初的位置与速度,那么协方差矩阵可以初始化为一个对角线元素是B 的矩阵,B 取一个合适的比较大的数。
P 0 | 0 = [ B 0 0 B ] {\displaystyle {\textbf {P}}_{0|0}={\begin{bmatrix}B&0\\0&B\end{bmatrix}}} 此时,与使用模型中已有信息相比,滤波器更倾向于使用初次测量值的信息。
推导后验协方差矩阵
编辑
按照上边的定义,从误差协方差P k | k {\displaystyle {\textbf {P}}_{k|k}} 开始推导如下:
P k | k = cov ( x k − x ^ k | k ) {\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k})} 代入x ^ k | k {\displaystyle {\hat {\textbf {x}}}_{k|k}}
P k | k = cov ( x k − ( x ^ k | k − 1 + K k y ~ k ) ) {\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}))} 再代入 y ~ k {\displaystyle {\tilde {\textbf {y}}}_{k}}
P k | k = cov ( x k − ( x ^ k | k − 1 + K k ( z k − H k x ^ k | k − 1 ) ) ) {\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}({\textbf {z}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1})))} 与z k {\displaystyle {\textbf {z}}_{k}}
P k | k = cov ( x k − ( x ^ k | k − 1 + K k ( H k x k + v k − H k x ^ k | k − 1 ) ) ) {\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}({\textbf {H}}_{k}{\textbf {x}}_{k}+{\textbf {v}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1})))} 整理误差向量,得
P k | k = cov ( ( I − K k H k ) ( x k − x ^ k | k − 1 ) − K k v k ) {\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}((I-{\textbf {K}}_{k}{\textbf {H}}_{k})({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k-1})-{\textbf {K}}_{k}{\textbf {v}}_{k})} 因为测量误差v k 与其他项是非相关的,因此有
P k | k = cov ( ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) ) + cov ( K k v k ) {\displaystyle \mathbf {P} _{k|k}={\textrm {cov}}((I-\mathbf {K} _{k}\mathbf {H} _{k})(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k-1}))+{\textrm {cov}}(\mathbf {K} _{k}\mathbf {v} _{k})} 利用协方差矩阵 的性质,此式可以写作
P k | k = ( I − K k H k ) cov ( x k − x ^ k | k − 1 ) ( I − K k H k ) T + K k cov ( v k ) K k T {\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textrm {cov}}({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k-1})(I-{\textbf {K}}_{k}{\textbf {H}}_{k})^{T}+{\textbf {K}}_{k}{\textrm {cov}}({\textbf {v}}_{k}){\textbf {K}}_{k}^{T}} 使用不变量P k |k -1 以及R k 的定义这一项可以写作
:
P k | k = ( I − K k H k ) P k | k − 1 ( I − K k H k ) T + K k R k K k T {\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}(I-{\textbf {K}}_{k}{\textbf {H}}_{k})^{T}+{\textbf {K}}_{k}{\textbf {R}}_{k}{\textbf {K}}_{k}^{T}}
这一公式对于任何卡尔曼增益K k 都成立。如果K k 是最优卡尔曼增益,则可以进一步简化,请见下文。
最优卡尔曼增益的推导
编辑
卡尔曼滤波器是一个最小均方误差 估计器,后验状态误差估计(英文:a posteriori state estimate)是
x k − x ^ k | k {\displaystyle {\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k}} 最小化这个矢量幅度平方的期望值,E [ | x k − x ^ k | k | 2 ] {\displaystyle {\textrm {E}}[|{\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k}|^{2}]} ,这等同于最小化后验估计协方差矩阵P k |k 的迹 (trace)。将上面方程中的项展开、抵消,得到:
P k | k {\displaystyle {\textbf {P}}_{k|k}}
= P k | k − 1 − K k H k P k | k − 1 − P k | k − 1 H k T K k T + K k ( H k P k | k − 1 H k T + R k ) K k T {\displaystyle ={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}-{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}+{\textbf {K}}_{k}({\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}){\textbf {K}}_{k}^{T}}
= P k | k − 1 − K k H k P k | k − 1 − P k | k − 1 H k T K k T + K k S k K k T {\displaystyle ={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}-{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}+{\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{T}}
当矩阵导数 是0的时候得到P k |k 的迹 (trace)的最小值:
d tr ( P k | k ) d K k = − 2 ( H k P k | k − 1 ) T + 2 K k S k = 0 {\displaystyle {\frac {d\;{\textrm {tr}}({\textbf {P}}_{k|k})}{d\;{\textbf {K}}_{k}}}=-2({\textbf {H}}_{k}{\textbf {P}}_{k|k-1})^{T}+2{\textbf {K}}_{k}{\textbf {S}}_{k}=0} 此处须用到一个常用的式子,如下:
d tr ( BAC ) d A = B T C T {\displaystyle {\frac {d\;{\textrm {tr}}({\textbf {BAC}})}{d\;{\textbf {A}}}}=B^{T}C^{T}}
从这个方程解出卡尔曼增益K k :
K k S k = ( H k P k | k − 1 ) T = P k | k − 1 H k T {\displaystyle {\textbf {K}}_{k}{\textbf {S}}_{k}=({\textbf {H}}_{k}{\textbf {P}}_{k|k-1})^{T}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}} K k = P k | k − 1 H k T S k − 1 {\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}} 这个增益称为最优卡尔曼增益 ,在使用时得到最小均方误差 。
后验误差协方差公式的化简
编辑
在卡尔曼增益等于上面导出的最优值时,计算后验协方差的公式可以进行简化。在卡尔曼增益公式两侧的右边都乘以S k K k T 得到
K k S k K k T = P k | k − 1 H k T K k T {\displaystyle {\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{T}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}} 根据上面后验误差协方差展开公式,
P k | k = P k | k − 1 − K k H k P k | k − 1 − P k | k − 1 H k T K k T + K k S k K k T {\displaystyle {\textbf {P}}_{k|k}={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}-{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}+{\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{T}} 最后两项可以抵消,得到
P k | k = P k | k − 1 − K k H k P k | k − 1 = ( I − K k H k ) P k | k − 1 {\displaystyle {\textbf {P}}_{k|k}={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}} .这个公式的计算比较简单,所以实际中总是使用这个公式,但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性 出现问题,或者特意使用非最优卡尔曼增益,那么就不能使用这个简化;必须使用上面导出的后验误差协方差公式。
与递归贝叶斯估计之间的关系
编辑
假设真正的状态是无法观察的马尔可夫过程 ,测量结果是从隐性马尔可夫模型观察到的状态。
根据马尔可夫假设,真正的状态仅受最近一个状态影响而与其它以前状态无关。
p ( x k | x 0 , … , x k − 1 ) = p ( x k | x k − 1 ) {\displaystyle p({\textbf {x}}_{k}|{\textbf {x}}_{0},\dots ,{\textbf {x}}_{k-1})=p({\textbf {x}}_{k}|{\textbf {x}}_{k-1})} 与此类似,在时刻k 测量只与当前状态有关而与其它状态无关。
p ( z k | x 0 , … , x k ) = p ( z k | x k ) {\displaystyle p({\textbf {z}}_{k}|{\textbf {x}}_{0},\dots ,{\textbf {x}}_{k})=p({\textbf {z}}_{k}|{\textbf {x}}_{k})} 根据这些假设,隐性马尔可夫模型所有状态的概率分布可以简化为:
p ( x 0 , … , x k , z 1 , … , z k ) = p ( x 0 ) ∏ i = 1 k p ( z i | x i ) p ( x i | x i − 1 ) {\displaystyle p({\textbf {x}}_{0},\dots ,{\textbf {x}}_{k},{\textbf {z}}_{1},\dots ,{\textbf {z}}_{k})=p({\textbf {x}}_{0})\prod _{i=1}^{k}p({\textbf {z}}_{i}|{\textbf {x}}_{i})p({\textbf {x}}_{i}|{\textbf {x}}_{i-1})} 然而,当卡尔曼滤波器用来估计状态x 时,感兴趣的几率分布,是基于目前为止所有个测量值来得到的当前状态之几率分布
p ( x k | Z k − 1 ) = ∫ p ( x k | x k − 1 ) p ( x k − 1 | Z k − 1 ) d x k − 1 {\displaystyle p({\textbf {x}}_{k}|{\textbf {Z}}_{k-1})=\int p({\textbf {x}}_{k}|{\textbf {x}}_{k-1})p({\textbf {x}}_{k-1}|{\textbf {Z}}_{k-1})\,d{\textbf {x}}_{k-1}} 信息滤波器
编辑
在信息滤波器或逆共变异数滤波器中,估计的共变异数和估计状态分别由信息矩阵 和信息 向量代替。 这些定义为:
Y k ∣ k = P k ∣ k − 1 y ^ k ∣ k = P k ∣ k − 1 x ^ k ∣ k {\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {P} _{k\mid k}^{-1}\\{\hat {\mathbf {y} }}_{k\mid k}&=\mathbf {P} _{k\mid k}^{-1}{\hat {\mathbf {x} }}_{k\mid k}\end{aligned}}} 同样,预测的共变异数和状态具有等效的信息形式,定义为:
Y k ∣ k − 1 = P k ∣ k − 1 − 1 y ^ k ∣ k − 1 = P k ∣ k − 1 − 1 x ^ k ∣ k − 1 {\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k-1}&=\mathbf {P} _{k\mid k-1}^{-1}\\{\hat {\mathbf {y} }}_{k\mid k-1}&=\mathbf {P} _{k\mid k-1}^{-1}{\hat {\mathbf {x} }}_{k\mid k-1}\end{aligned}}} 以及测量共变异数和测量向量,它们定义为:
I k = H k T R k − 1 H k i k = H k T R k − 1 z k {\displaystyle {\begin{aligned}\mathbf {I} _{k}&=\mathbf {H} _{k}^{\textsf {T}}\mathbf {R} _{k}^{-1}\mathbf {H} _{k}\\\mathbf {i} _{k}&=\mathbf {H} _{k}^{\textsf {T}}\mathbf {R} _{k}^{-1}\mathbf {z} _{k}\end{aligned}}} 信息更新现在变得微不足道了。
Y k ∣ k = Y k ∣ k − 1 + I k y ^ k ∣ k = y ^ k ∣ k − 1 + i k {\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {Y} _{k\mid k-1}+\mathbf {I} _{k}\\{\hat {\mathbf {y} }}_{k\mid k}&={\hat {\mathbf {y} }}_{k\mid k-1}+\mathbf {i} _{k}\end{aligned}}} 信息过滤器的主要优点是,只需将其测量信息矩阵和向量相加即可在每个时间步长过滤N个测量值。
Y k ∣ k = Y k ∣ k − 1 + ∑ j = 1 N I k , j y ^ k ∣ k = y ^ k ∣ k − 1 + ∑ j = 1 N i k , j {\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {Y} _{k\mid k-1}+\sum _{j=1}^{N}\mathbf {I} _{k,j}\\{\hat {\mathbf {y} }}_{k\mid k}&={\hat {\mathbf {y} }}_{k\mid k-1}+\sum _{j=1}^{N}\mathbf {i} _{k,j}\end{aligned}}} 为了预测信息过滤器,可以将信息矩阵和向量转换回它们的状态空间等效项,或者可以使用信息空间预测。
M k = [ F k − 1 ] T Y k − 1 ∣ k − 1 F k − 1 C k = M k [ M k + Q k − 1 ] − 1 L k = I − C k Y k ∣ k − 1 = L k M k L k T + C k Q k − 1 C k T y ^ k ∣ k − 1 = L k [ F k − 1 ] T y ^ k − 1 ∣ k − 1 {\displaystyle {\begin{aligned}\mathbf {M} _{k}&=\left[\mathbf {F} _{k}^{-1}\right]^{\textsf {T}}\mathbf {Y} _{k-1\mid k-1}\mathbf {F} _{k}^{-1}\\\mathbf {C} _{k}&=\mathbf {M} _{k}\left[\mathbf {M} _{k}+\mathbf {Q} _{k}^{-1}\right]^{-1}\\\mathbf {L} _{k}&=\mathbf {I} -\mathbf {C} _{k}\\\mathbf {Y} _{k\mid k-1}&=\mathbf {L} _{k}\mathbf {M} _{k}\mathbf {L} _{k}^{\textsf {T}}+\mathbf {C} _{k}\mathbf {Q} _{k}^{-1}\mathbf {C} _{k}^{\textsf {T}}\\{\hat {\mathbf {y} }}_{k\mid k-1}&=\mathbf {L} _{k}\left[\mathbf {F} _{k}^{-1}\right]^{\textsf {T}}{\hat {\mathbf {y} }}_{k-1\mid k-1}\end{aligned}}} 如果F和Q是非时变的,则可以将这些值缓存起来,并且F和Q必须是可逆的。
频率加权卡尔曼滤波器
编辑
非线性滤波器
编辑
基本卡尔曼滤波器(The basic Kalman filter)是限制在线性的假设之下。然而,大部分非平凡的(non-trivial)的系统都是非线性系统。其中的“非线性性质”(non-linearity)可能是伴随存在过程模型(process model)中或观测模型(observation model)中,或者两者兼有之。
扩展卡尔曼滤波器
编辑
在扩展卡尔曼滤波器(Extended Kalman Filter,简称EKF)中状态转换和观测模型不需要是状态的线性函数,可替换为(可微的)函数。
x k = f ( x k − 1 , u k , w k ) {\displaystyle {\textbf {x}}_{k}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k},{\textbf {w}}_{k})} z k = h ( x k , v k ) {\displaystyle {\textbf {z}}_{k}=h({\textbf {x}}_{k},{\textbf {v}}_{k})} 函数f 可以用来从过去的估计值中计算预测的状态,相似的,函数h 可以用来以预测的状态计算预测的测量值。然而f 和h 不能直接的应用在协方差中,取而代之的是计算偏导矩阵(Jacobian )。
在每一步中使用当前的估计状态计算Jacobian矩阵,这几个矩阵可以用在卡尔曼滤波器的方程中。这个过程,实质上将非线性的函数在当前估计值处线性化了。
这样一来,卡尔曼滤波器的等式为:
预测
x ^ k | k − 1 = f ( x k − 1 , u k , 0 ) {\displaystyle {\hat {\textbf {x}}}_{k|k-1}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k},0)} P k | k − 1 = F k P k − 1 | k − 1 F k T + Q k {\displaystyle {\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}} 使用Jacobians矩阵更新模型
F k = ∂ f ∂ x | x ^ k − 1 | k − 1 , u k {\displaystyle {\textbf {F}}_{k}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k}}} H k = ∂ h ∂ x | x ^ k | k − 1 {\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}} 更新
y ~ k = z k − h ( x ^ k | k − 1 , 0 ) {\displaystyle {\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-h({\hat {\textbf {x}}}_{k|k-1},0)} S k = H k P k | k − 1 H k T + R k {\displaystyle {\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}} K k = P k | k − 1 H k T S k − 1 {\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}} x ^ k | k = x ^ k | k − 1 + K k y ~ k {\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}} P k | k = ( I − K k H k ) P k | k − 1 {\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}} 预测
如同扩展卡尔曼滤波器(EKF)一样, UKF的预测过程可以独立于UKF的更新过程之外,与一个线性的(或者确实是扩展卡尔曼滤波器的)更新过程合并来使用;或者,UKF的预测过程与更新过程在上述中地位互换亦可。
卡尔曼-布西滤波器
编辑
卡尔曼-布西滤波器(Kalman-Bucy filter,以Richard Snowden Bucy命名)是Kalman滤波器的连续时间版本。
它基于状态空间模型
d d t x ( t ) = F ( t ) x ( t ) + B ( t ) u ( t ) + w ( t ) z ( t ) = H ( t ) x ( t ) + v ( t ) {\displaystyle {\begin{aligned}{\frac {d}{dt}}\mathbf {x} (t)&=\mathbf {F} (t)\mathbf {x} (t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {w} (t)\\\mathbf {z} (t)&=\mathbf {H} (t)\mathbf {x} (t)+\mathbf {v} (t)\end{aligned}}} 其中 Q ( t ) {\displaystyle \mathbf {Q} (t)} 和 R ( t ) {\displaystyle \mathbf {R} (t)} 分别代表两个白噪声项 w ( t ) {\displaystyle \mathbf {w} (t)} 和 v ( t ) {\displaystyle \mathbf {v} (t)} 的强度(或更准确地说:功率谱密度-PSD-矩阵)。
该滤波器由两个微分方程组成,一个用于状态估计,一个用于共变异数:
d d t x ^ ( t ) = F ( t ) x ^ ( t ) + B ( t ) u ( t ) + K ( t ) ( z ( t ) − H ( t ) x ^ ( t ) ) d d t P ( t ) = F ( t ) P ( t ) + P ( t ) F T ( t ) + Q ( t ) − K ( t ) R ( t ) K T ( t ) {\displaystyle {\begin{aligned}{\frac {d}{dt}}{\hat {\mathbf {x} }}(t)&=\mathbf {F} (t){\hat {\mathbf {x} }}(t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {K} (t)\left(\mathbf {z} (t)-\mathbf {H} (t){\hat {\mathbf {x} }}(t)\right)\\{\frac {d}{dt}}\mathbf {P} (t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} ^{\textsf {T}}(t)+\mathbf {Q} (t)-\mathbf {K} (t)\mathbf {R} (t)\mathbf {K} ^{\textsf {T}}(t)\end{aligned}}} 卡尔曼增益由
K ( t ) = P ( t ) H T ( t ) R − 1 ( t ) {\displaystyle \mathbf {K} (t)=\mathbf {P} (t)\mathbf {H} ^{\textsf {T}}(t)\mathbf {R} ^{-1}(t)} 注意,在此表达式中,对于 K ( t ) {\displaystyle \mathbf {K} (t)} ,观察噪声 R ( t ) {\displaystyle \mathbf {R} (t)} 的共变异数同时表示预测误差(或创新)y ~ ( t ) = z ( t ) − H ( t ) x ^ ( t ) {\displaystyle {\tilde {\mathbf {y} }}(t)=\mathbf {z} (t)-\mathbf {H} (t){\hat {\mathbf {x} }}(t)} 的共变异数; 这些共变异数仅在连续时间的情况下才相等。
离散时间卡尔曼滤波的预测步骤和更新步骤之间的区别在连续时间内不存在。
用于共变异数的第二个微分方程是Riccati方程 的一个示例。
混合型卡尔曼滤波器
编辑
大多数物理系统表示为连续时间模型,而离散时间测量则经常通过数字处理器进行状态估计。 因此,系统模型和测量模型由下式给出:
x ˙ ( t ) = F ( t ) x ( t ) + B ( t ) u ( t ) + w ( t ) , w ( t ) ∼ N ( 0 , Q ( t ) ) z k = H k x k + v k , v k ∼ N ( 0 , R k ) {\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=\mathbf {F} (t)\mathbf {x} (t)+\mathbf {B} (t)\mathbf {u} (t)+\mathbf {w} (t),&\mathbf {w} (t)&\sim N\left(\mathbf {0} ,\mathbf {Q} (t)\right)\\\mathbf {z} _{k}&=\mathbf {H} _{k}\mathbf {x} _{k}+\mathbf {v} _{k},&\mathbf {v} _{k}&\sim N(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}} 当
x k = x ( t k ) {\displaystyle \mathbf {x} _{k}=\mathbf {x} (t_{k})} .
x ^ 0 ∣ 0 = E [ x ( t 0 ) ] , P 0 ∣ 0 = Var [ x ( t 0 ) ] {\displaystyle {\hat {\mathbf {x} }}_{0\mid 0}=E\left[\mathbf {x} (t_{0})\right],\mathbf {P} _{0\mid 0}=\operatorname {Var} \left[\mathbf {x} \left(t_{0}\right)\right]}
x ^ ˙ ( t ) = F ( t ) x ^ ( t ) + B ( t ) u ( t ) , with x ^ ( t k − 1 ) = x ^ k − 1 ∣ k − 1 ⇒ x ^ k ∣ k − 1 = x ^ ( t k ) P ˙ ( t ) = F ( t ) P ( t ) + P ( t ) F ( t ) T + Q ( t ) , with P ( t k − 1 ) = P k − 1 ∣ k − 1 ⇒ P k ∣ k − 1 = P ( t k ) {\displaystyle {\begin{aligned}{\dot {\hat {\mathbf {x} }}}(t)&=\mathbf {F} (t){\hat {\mathbf {x} }}(t)+\mathbf {B} (t)\mathbf {u} (t){\text{, with }}{\hat {\mathbf {x} }}\left(t_{k-1}\right)={\hat {\mathbf {x} }}_{k-1\mid k-1}\\\Rightarrow {\hat {\mathbf {x} }}_{k\mid k-1}&={\hat {\mathbf {x} }}\left(t_{k}\right)\\{\dot {\mathbf {P} }}(t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{\textsf {T}}+\mathbf {Q} (t){\text{, with }}\mathbf {P} \left(t_{k-1}\right)=\mathbf {P} _{k-1\mid k-1}\\\Rightarrow \mathbf {P} _{k\mid k-1}&=\mathbf {P} \left(t_{k}\right)\end{aligned}}} 预测方程式是从连续时间卡尔曼滤波器的方程式推导而得,而无需进行测量更新,例如: K ( t ) = 0 {\displaystyle \mathbf {K} (t)=0} 。预测状态和共变异数分别通过求解一组初始值等于上一步估计值的微分方程组来计算。
在线性非时变系统 的情况下,可以使用矩阵指数 将连续时间动态精确地离散化 为离散时间系统。
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) P k ∣ k = ( I − K k H k ) P k ∣ k − 1 {\displaystyle {\begin{aligned}\mathbf {K} _{k}&=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\left(\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}+\mathbf {R} _{k}\right)^{-1}\\{\hat {\mathbf {x} }}_{k\mid k}&={\hat {\mathbf {x} }}_{k\mid k-1}+\mathbf {K} _{k}\left(\mathbf {z} _{k}-\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1}\right)\\\mathbf {P} _{k\mid k}&=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\mathbf {P} _{k\mid k-1}\end{aligned}}} 更新方程与离散时间卡尔曼滤波器的更新方程相同。
外部链接
编辑
参考文献
编辑
Gelb A., editor. Applied optimal estimation. MIT Press, 1974. Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME - Journal of Basic Engineering Vol. 82: pp. 35-45 (1960) Kalman, R. E., Bucy R. S., New Results in Linear Filtering and Prediction Theory, Transactions of the ASME - Journal of Basic Engineering Vol. 83: pp. 95-107 (1961) [JU97] Julier, Simon J. and Jeffery K. Uhlmann. A New Extension of the Kalman Filter to nonlinear Systems. In The Proceedings of AeroSense: The 11th International Symposium on Aerospace/Defense Sensing,Simulation and Controls, Multi Sensor Fusion, Tracking and Resource Management II, SPIE, 1997. Harvey, A.C. Forecasting, Structural Time Series Models and the Kalman Filter. Cambridge University Press, Cambridge, 1989. ^ Paul Zarchan; Howard Musoff. Fundamentals of Kalman Filtering: A Practical Approach . American Institute of Aeronautics and Astronautics, Incorporated. 2000 [2018-02-16 ] . ISBN 978-1-56347-455-2 . (原始内容存档 于2017-02-16).
^ Wolpert, Daniel; Ghahramani, Zoubin. Computational principles of movement neuroscience. Nature Neuroscience. 2000, 3 : 1212–7. PMID 11127840 . doi:10.1038/81497 .
^ Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering. 1960, 82 : 35. doi:10.1115/1.3662552 .