在《跟踪导论(六)》中阐述了卡尔曼滤波的基本定义和三大关键参数:卡尔曼增益k {\bf{k}}k、预测均方误差M {\bf{M}}M、状态转移矩阵F {\bf{F}}F释义的理解。基本定义如公式(1)所示。其中x ^ [ n ] {\bf{\hat x}}\left[ n \right]x^[n]表示卡尔曼滤波后的状态估计值、x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]表示状态观测值、x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]表示状态预测值。(状态包含位置和速度)。
x ^ [ n ] = x ˙ [ n ] + k ( x ~ [ n ] − x ˙ [ n ] ) ( 1 ) {\bf{\hat x}}\left[ n \right] = {\bf{\dot x}}\left[ n \right] + {\bf{k}}\left( {{\bf{\tilde x}}\left[ n \right] - {\bf{\dot x}}\left[ n \right]} \right)(1)x^[n]=x˙[n]+k(x~[n]−x˙[n])(1)
但是三大参数是如何动态迭代的呢?迭代的过程中卡尔曼滤波是如何逼近客观真实的呢?这个就必须揭开各参数之间联动的真相了。
在《跟踪导论(六)》中已经阐述了卡尔曼增益k {\bf{k}}k与预测均方误差M {\bf{M}}M的联动关系即:
k [ n ] = M [ n ] σ w [ n ] 2 + M [ n ] ( 2 ) {\bf{k}}\left[ n \right] = {{{\bf{M}}\left[ n \right]} \over {\sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{M}}\left[ n \right]}}(2)k[n]=σw[n]2+M[n]M[n](2)
这个联动关系表达的物理逻辑即:当预测均方误差M [ n ] {\bf{M}}\left[ n \right]M[n]过大时,状态预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]不太可信了,此时,卡尔曼增益k [ n ] {\bf{k}}\left[ n \right]k[n]将随之变大,公式(1)中的状态估计值x ^ [ n ] {\bf{\hat x}}\left[ n \right]x^[n]将更依赖于观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n],使之对状态预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]进行修正;当预测均方误差M [ n ] {\bf{M}}\left[ n \right]M[n]很小时,状态预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]可信度高,此时,卡尔曼增益k [ n ] {\bf{k}}\left[ n \right]k[n]将随之变小,公式(1)中的状态估计值x ^ [ n ] {\bf{\hat x}}\left[ n \right]x^[n]将更依赖于状态预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n],从而减少状态观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]对状态预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]的修正比例。这个联动关系的物理逻辑是十分直观的,那么预测均方误差M {\bf{M}}M与状态转移矩阵F {\bf{F}}F的联动关系又是怎样的呢?
一、预测均方误差M {\bf{M}}M与状态转移矩阵F {\bf{F}}F的联动真相
回顾《跟踪导论(六)》中阐述的状态转移矩阵F {\bf{F}}F的物理意义是根据目标的运动状态用上一时刻的状态估计值x ^ [ n − 1 ] {\bf{\hat x}}\left[ {n - 1} \right]x^[n−1]对当下时刻状态预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]进行预测的数学模型,即:
x ˙ [ n ] = F ⋅ x ^ [ n − 1 ] ( 3 ) {\bf{\dot x}}\left[ n \right] = {\bf{F}} \cdot {\bf{\hat x}}\left[ {n - 1} \right](3)x˙[n]=F⋅x^[n−1](3)
因此,根据预测均方误差M {\bf{M}}M的物理来源可知,当下时刻的预测均方误差M {\bf{M}}M应该也可以由状态转移矩阵F {\bf{F}}F和过程噪声q {\bf{q}}q得到。过程噪声q {\bf{q}}q是预测阶段独有的干扰,比如跟踪无人机时,气流的随机扰动会让无人机偏离预期轨迹;再比如雷达设备本身的电路噪声,会在基于历史数据推算预测值时,悄悄叠加微小偏差。因此表面上预测均方误差M {\bf{M}}M与状态转移矩阵F {\bf{F}}F的联动关系可表述为:
M [ n ] = F ⋅ M [ n − 1 ] ⋅ F T + g ⋅ q ⋅ g T ( 4 ) {\bf{M}}\left[ n \right] = {\bf{F}} \cdot {\bf{M}}\left[ {n - 1} \right] \cdot {{\bf{F}}^T} + {\bf{g}} \cdot {\bf{q}} \cdot {{\bf{g}}^T}(4)M[n]=F⋅M[n−1]⋅FT+g⋅q⋅gT(4)
然而,这里有一个关键问题,即M {\bf{M}}M的本质是雷达在当下时刻t n {t_n}tn的预测均方误差,而状态转移矩阵F {\bf{F}}F和过程噪声q {\bf{q}}q都是以雷达探测时自己设定的目标运动模型来进行构建的,也就是说目标在运动过程中改变了运动模型,那么其由不变的状态转移矩阵F {\bf{F}}F和过程噪声q {\bf{q}}q表征当下时刻t n {t_n}tn预测均方误差M {\bf{M}}M的逻辑就不能完全成立,其必然会引来因目标当下运动状态改变而导致的更大误差。因此,实际上F ⋅ M [ n − 1 ] ⋅ F T + g ⋅ q ⋅ g T {\bf{F}} \cdot {\bf{M}}\left[ {n - 1} \right] \cdot {{\bf{F}}^T} + {\bf{g}} \cdot {\bf{q}} \cdot {{\bf{g}}^T}F⋅M[n−1]⋅FT+g⋅q⋅gT并不能完全表征雷达在当下时刻的预测均方误差,其仍然是一种预测状态,即根据上一时刻t n − 1 {t_{n - 1}}tn−1的运动模型确定的状态转移矩阵F {\bf{F}}F和过程噪声q {\bf{q}}q来预测的当下时刻的预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]。因此公式(4)应改为:
M ˙ [ n ] = F ⋅ M ^ [ n − 1 ] ⋅ F T + g ⋅ q ⋅ g T ( 5 ) {\bf{\dot M}}\left[ n \right] = {\bf{F}} \cdot {\bf{\hat M}}\left[ {n - 1} \right] \cdot {{\bf{F}}^T} + {\bf{g}} \cdot {\bf{q}} \cdot {{\bf{g}}^T}(5)M˙[n]=F⋅M^[n−1]⋅FT+g⋅q⋅gT(5)
二、卡尔曼增益k {\bf{k}}k与预测均方误差M {\bf{M}}M的联动真相
根据上面的阐述,公式(2)应表示为:
k [ n ] = M ˙ [ n ] σ w [ n ] 2 + M [ n ] ( 6 ) {\bf{k}}\left[ n \right] = {{{\bf{\dot M}}\left[ n \right]} \over {\sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{M}}\left[ n \right]}}(6)k[n]=σw[n]2+M[n]M˙[n](6)
公式(6)将公式(2)中的分子M [ n ] {\bf{M}}\left[ n \right]M[n]换成了M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n],但分母没有替换,这是因为分母中的σ w [ n ] 2 \sigma _{{\bf{w}}\left[ n \right]}^2σw[n]2仍是雷达无法观测到的一个值,而σ w [ n ] 2 \sigma _{{\bf{w}}\left[ n \right]}^2σw[n]2和M [ n ] {\bf{M}}\left[ n \right]M[n]共同组成了另一个雷达可观测的值,这点我们放在后面阐述。这里就有两个预测,“预测均方误差M {\bf{M}}M”一词中的预测是指这个变量M {\bf{M}}M的含义是表征状态预测值x ˙ {\bf{\dot x}}x˙与状态客观真实值x {\bf{x}}x的均方误差;M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]变量中含有的预测是指M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]变量不是精准表征状态预测值x ˙ {\bf{\dot x}}x˙与状态客观真实值x {\bf{x}}x的均方误差,而是这个值是根据雷达自身设定的目标运动状态进行预测得到的。两个预测之间的关系示意图如图1所示,也就是说我们在当下时刻得到的预测均方误差M ^ [ n − 1 ] {\bf{\hat M}}\left[ {n - 1} \right]M^[n−1]并不能表征我们设定的状态转移矩阵F {\bf{F}}F和过程噪声q {\bf{q}}q与客观真实之间的差距,而状态转移矩阵F {\bf{F}}F和过程噪声q {\bf{q}}q与客观真实之间的差距是我们对目标运动状态估计得准与不准之间的重要反映,通过公式(6)可知,只有让预测均方误差尽量准确地逼近真实均方误差,才能得到尽量符合客观现实的卡尔曼滤波增益k {\bf{k}}k,也就是对观测值和预测值进行符合客观事实的信任。当然,在当下时刻t n {t_n}tn,我们只能通过公式(5)得到由上一时刻设定的运动模型预测得到的预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n],也就只能用这个预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]计算卡尔曼滤波增益k [ n ] {\bf{k}}\left[ n \right]k[n](公式(6)),但是,在我们获取了当下时刻的最新观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]后,我们就可以通过最新的观测值来修正我们之前通过设定的运动模型预测得到的预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n],从而得到更靠近客观真实情况的预测均方误差M ^ [ n ] {\bf{\hat M}}\left[ n \right]M^[n]。
如何利用最新的观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]来修正我们之前通过设定的运动模型预测得到的预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]呢?这里我们来观察公式(2)中的分母,即σ w [ n ] 2 + M [ n ] \sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{M}}\left[ n \right]σw[n]2+M[n]。观测噪声功率σ w [ n ] 2 \sigma _{{\bf{w}}\left[ n \right]}^2σw[n]2,观测噪声功率σ w [ n ] 2 \sigma _{{\bf{w}}\left[ n \right]}^2σw[n]2表征的是观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]与客观真实值x [ n ] {\bf{x}}\left[ n \right]x[n]之间的均方误差,而预测均方误差M [ n ] {\bf{M}}\left[ n \right]M[n]表征的是预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]与客观真实值x [ n ] {\bf{x}}\left[ n \right]x[n]之间的均方误差,因此可以知道观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]与预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]之间的均方误差可以抽象地等效于观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]与客观真实值x [ n ] {\bf{x}}\left[ n \right]x[n]之间的均方误差加上预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]与客观真实值x [ n ] {\bf{x}}\left[ n \right]x[n]之间的均方误差,即σ ~ 2 = σ w [ n ] 2 + M [ n ] {\tilde \sigma ^2} = \sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{M}}\left[ n \right]σ~2=σw[n]2+M[n]表征的是观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]与预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]之间的均方误差。而观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]和预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]都是雷达可观测到的值。也就是说公式(6)在计算时分子用的是通过设定的运动模型预测得到的预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n],而分母用的是实际观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]与预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]的均方误差。此时我们观察公式(6)可知,若我们通过设定的运动模型预测得到的预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]小于由真实运动模型预测得到的预测均方误差M [ n ] {\bf{M}}\left[ n \right]M[n](M ˙ [ n ] < M [ n ] {\bf{\dot M}}\left[ n \right] < {\bf{M}}\left[ n \right]M˙[n]<M[n]我们对误差的预测过于乐观),那么观测值实际观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]与预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]的均方误差σ ~ 2 = σ w [ n ] 2 + M [ n ] > σ w [ n ] 2 + M ˙ [ n ] {\tilde \sigma ^2} = \sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{M}}\left[ n \right] > \sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{\dot M}}\left[ n \right]σ~2=σw[n]2+M[n]>σw[n]2+M˙[n]就会偏大,此时卡尔曼滤波增益k {\bf{k}}k就会偏小;若我们通过设定的运动模型预测得到的预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]大于由真实运动模型预测得到的预测均方误差M [ n ] {\bf{M}}\left[ n \right]M[n](M ˙ [ n ] > M [ n ] {\bf{\dot M}}\left[ n \right] > {\bf{M}}\left[ n \right]M˙[n]>M[n]我们对误差的预测过于悲观),那么观测值实际观测值x ~ [ n ] {\bf{\tilde x}}\left[ n \right]x~[n]与预测值x ˙ [ n ] {\bf{\dot x}}\left[ n \right]x˙[n]的均方误差σ ~ 2 = σ w [ n ] 2 + M [ n ] < σ w [ n ] 2 + M ˙ [ n ] {\tilde \sigma ^2} = \sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{M}}\left[ n \right] < \sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{\dot M}}\left[ n \right]σ~2=σw[n]2+M[n]<σw[n]2+M˙[n]就会偏小,那我们得到的卡尔曼滤波增益k [ n ] {\bf{k}}\left[ n \right]k[n]就偏大,如图2所示。
那么此时我们就可以用卡尔曼滤波增益的值k [ n ] {\bf{k}}\left[ n \right]k[n]的反面来对预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]进行修正和制约,让预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]偏大的时候经过修正则减小,在预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]偏小的时候经过修正则增大,从而达到收敛。那么要用卡尔曼滤波增益的值k [ n ] {\bf{k}}\left[ n \right]k[n]的反面来对预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]进行修正的最直观的方式即:
M ^ [ n ] = ( I − k [ n ] ) M ˙ [ n ] ( 7 ) {\bf{\hat M}}\left[ n \right] = \left( {{\bf{I}} - {\bf{k}}\left[ n \right]} \right){\bf{\dot M}}\left[ n \right](7)M^[n]=(I−k[n])M˙[n](7)
如此我们便得到了卡尔曼滤波算法各参数之间的所有联动关系:
M ˙ [ n ] = F ⋅ M ^ [ n − 1 ] ⋅ F T + g ⋅ q ⋅ g T ( 8 ) {\bf{\dot M}}\left[ n \right] = {\bf{F}} \cdot {\bf{\hat M}}\left[ {n - 1} \right] \cdot {{\bf{F}}^T} + {\bf{g}} \cdot {\bf{q}} \cdot {{\bf{g}}^T}(8)M˙[n]=F⋅M^[n−1]⋅FT+g⋅q⋅gT(8)
k [ n ] = M ˙ [ n ] σ w [ n ] 2 + M ˙ [ n ] ( σ ~ 2 = σ w [ n ] 2 ) ( 9 ) {\bf{k}}\left[ n \right] = {{{\bf{\dot M}}\left[ n \right]} \over {\sigma _{{\bf{w}}\left[ n \right]}^2 + {\bf{\dot M}}\left[ n \right]}}\left( {{{\tilde \sigma }^2} = \sigma _{{\bf{w}}\left[ n \right]}^2} \right)(9)k[n]=σw[n]2+M˙[n]M˙[n](σ~2=σw[n]2)(9)
M ^ [ n ] = ( I − k [ n ] ) M ˙ [ n ] ( 10 ) {\bf{\hat M}}\left[ n \right] = \left( {{\bf{I}} - {\bf{k}}\left[ n \right]} \right){\bf{\dot M}}\left[ n \right](10)M^[n]=(I−k[n])M˙[n](10)
观察该公式,状态转移矩阵F {\bf{F}}F与过程噪声状态转移矩阵q {\bf{q}}q均是由根据雷达自身设定的目标运动状态模型构建的,独立于目标的真实运动状态和每一时刻的观测值,但是状态转移矩阵F {\bf{F}}F与过程噪声状态转移矩阵q {\bf{q}}q是否与目标真实运动状态贴合会作用在预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]上,而预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]的准确与否会表征在观测值与预测值的差距上。因此利用观测值与预测值的均方误差σ ~ 2 {\tilde \sigma ^2}σ~2构建了卡尔曼滤波增益k [ n ] {\bf{k}}\left[ n \right]k[n]与预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]之间的关系,使得卡尔曼滤波增益k [ n ] {\bf{k}}\left[ n \right]k[n]的大小会随着预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]的偏差而改变,从而利用卡尔曼滤波增益k [ n ] {\bf{k}}\left[ n \right]k[n]不断修正和约束预测均方误差M ˙ [ n ] {\bf{\dot M}}\left[ n \right]M˙[n]使之紧紧贴合目标真实的运动状态而不断修正。整个过程中雷达只需设定好目标运动模型、从而确定状态转移矩阵F {\bf{F}}F与过程噪声状态转移矩阵q {\bf{q}}q,就可利用每一时刻的观测值不断修正因设定的目标运动模型和目标真实运动模型不同带来的滤波误差。这就是卡尔曼滤波的核心要以。