信息融合状态估计卡尔曼滤波

信息融合状态估计卡尔曼滤波1第一页，共六十九页，2022年，8月28日状态估计的主要内容应用：通过数学方法寻求与观测数据最佳拟合的状态向量。1、确定运动目标的当前位置与速度；2、确定运动目标的未来位置与速度；3、确定运动目标的固有特征或特征参数。2第二页，共六十九页，2022年，8月28日状态估计主要内容：位置与速度估计。位置估计：距离、方位和高度或仰角的估计；速度估计：速度、加速度估计。3第三页，共六十九页，2022年，8月28日状态估计的主要方法1、α-β滤波2、α-β-γ滤波3、卡尔曼滤波这些方法针对匀速或匀加速目标提出，如目标真实运动与采用的目标模型不一致，滤波器发散。4第四页，共六十九页，2022年，8月28日算法的改进及适应性状态估计难点：机动目标的跟踪

1、自适应α-β滤波和自适应Kalman滤波均改善对机动目标的跟踪能力。

2、扩展Kalman滤波针对卡尔曼滤波在笛卡儿坐标系中才能使用的局限而提出。5第五页，共六十九页，2022年，8月28日卡尔曼滤波器卡尔曼滤波器的应用:通信、雷达、导航、自动控制等领域；航天器的轨道计算、雷达目标跟踪、生产过程的自动控制等。6第六页，共六十九页，2022年，8月28日卡尔曼滤波器的应用特点对机动目标跟踪中具有良好的性能；为最佳估计并能够进行递推计算；只需当前的一个测量值和前一个采样周期的预测值就能进行状态估计。7第七页，共六十九页，2022年，8月28日卡尔曼滤波器的局限性卡尔曼滤波器解决运动目标或实体的状态估计问题时，动态方程和测量方程均为线性。8第八页，共六十九页，2022年，8月28日一、数字滤波器作估值器1、非递归估值器

2、递归估值器9第九页，共六十九页，2022年，8月28日1、非递归估值器采样平均估值器：采用时域分析方法在掺杂有噪声的测量信号中估计信号x。10第十页，共六十九页，2022年，8月28日根据数字信号处理我们知道，所谓非递归数字滤波器是一种只有前馈而没有反馈的滤波器，它的冲击脉冲响应是有限的，在许多领域有着广泛的应用。假定用zk表示观测值，

zk=x+nk

式中：x—恒定信号或称被估参量

nk—观测噪声采样

假定，E(x)=x0，D(x)=σ2x，E(nk)=0，E(n2k)=σ2n。11第十一页，共六十九页，2022年，8月28日

h1,h2,…,hm是滤波器的脉冲响应hj的采样，或称滤波器的加权系数。滤波器的输出当h1=h2=…=hm=1/m时，

该式表明，估计是用m个采样值的平均值作为被估参量x的近似值的，故称其为采样平均估值器。

12第十二页，共六十九页，2022年，8月28日估计的均方误差以Pε表示，有当i=j时δij=1，当i≠j时δij=0，有

最后得：

13第十三页，共六十九页，2022年，8月28日结论①估计值是用m个采样值的平均值作为被估参量x的近似值；②估值器的均方误差随着m的增加而减少；③该估值器是一个无偏估值器。14第十四页，共六十九页，2022年，8月28日2、递归估值器一阶递归估值器：

a为滤波器的加权系数，a<1。15第十五页，共六十九页，2022年，8月28日递归数字滤波器是一种带有反馈的滤波器，它有无限的脉冲响应，有阶数少的优点，但其暂态过程较长。关于信号和噪声的基本假设与非递归情况相同。上图给出的一阶递归滤波器输入输出信号关系如下：式中，zk与非递归情况相同；a是一个小于1的滤波器加权系数，如果它大于或等于1，该滤波器就不稳定了。

16第十六页，共六十九页，2022年，8月28日k时刻的输出：

yk=ak-1z1+ak-2z2+…+azk-1+zk

将zk中的信号和噪声分开，并代入，有输出由于│a│＜1，故随着k值的增加，yk趋近于x/(1-a)。这样，如果以(1-a)yk作为x的估计值，则17第十七页，共六十九页，2022年，8月28日此时信号x和估值之间只差一个噪声项。当k值较大时，估值的均方误差而一次取样的均方误差

故上一结果的均方误差约为一次采样的（1-a）/（1+a）倍。

18第十八页，共六十九页，2022年，8月28日二、线性均方估计1、最优非递归估计（标量维纳滤波）2、递归估计19第十九页，共六十九页，2022年，8月28日

1.最优非递归估计非递归滤波器的估计值及其估计误差可分别表示为20第二十页，共六十九页，2022年，8月28日对m个参数逐一求导，令等于零，在均值为零的白噪声的情况下，可得到最小均方误差和估计：其中，b=σ2n/σ2x，在b<<m时，这种估计近似于采样平均。在噪声方差σ2n较大时，其性能明显优于非最佳情况。这种最小均方误差准则下的线性滤波，通常称作标量维纳滤波。

hj与非最优情况的不同，这里的滤波器的加权系数为21第二十一页，共六十九页，2022年，8月28日

2、由最优非递推估计导出递归估计由前可知，非递归估值器可以表示为

条件与前面相同。对k+1次取样，相应的估计量

相应的估计误差

22第二十二页，共六十九页，2022年，8月28日由b=σ2n/σ2x及hi(k)=1/（k+b），有

所以有

23第二十三页，共六十九页，2022年，8月28日于是,分成二项：将第一项同时乘、除一个bk，则24第二十四页，共六十九页，2022年，8月28日或

最后有

25第二十五页，共六十九页，2022年，8月28日最优递归估计器递推公式26第二十六页，共六十九页，2022年，8月28日最优递归估计器递推公式27第二十七页，共六十九页，2022年，8月28日

递推开始时的初始条件应满足：以使为最佳值。解之，得 ，这时的 如果E(x)=0，可从零开始递推运算，即28第二十八页，共六十九页，2022年，8月28日三、标量卡尔曼滤波器－时变信号主要作用：对掺杂有噪声的随机信号进行线性估计。29第二十九页，共六十九页，2022年，8月28日1、模型

1）信号模型设要估计的随机信号为由均值为0，方差为σ2w的白噪声激励的一个一阶递归过程，即信号对时间变化满足动态方程：x(k)=ax(k-1)+w(k-1)

式中，a——系统参数；

w(k-1)——白噪声采样。如果令x(0)=0，E［w(k)］=0，则

30第三十页，共六十九页，2022年，8月28日该过程称作一阶自回归过程。x(k)的均值和方差分别为：

自相关函数

31第三十一页，共六十九页，2022年，8月28日2）观测模型观测模型由下式给出：z(k)=cx(k)+v(k)式中：c——测量因子；

v(k)——E(·)=0，D(·)=σ2n的白噪声。最优递推估值器的信号和观测模型如图所示。

32第三十二页，共六十九页，2022年，8月28日最优递推估值器的信号和观测模型33第三十三页，共六十九页，2022年，8月28日2、标量卡尔曼滤波器由前将递归估计的形式写成：均方误差

分别对a(k)和b(k)求导，并令其等于0，求其最佳估计，得出a(k)与b(k)的关系：a(k)=a［1-cb(k)］最后有递归估值器：

34第三十四页，共六十九页，2022年，8月28日b(k)为滤波器增益

其中,

均方误差

对于给定的信号模型和观测模型，上述一组方程便称为一维标量卡尔曼滤波器，其结构如图所示。35第三十五页，共六十九页，2022年，8月28日标量卡尔曼滤波器结构

36第三十六页，共六十九页，2022年，8月28日

3、标量卡尔曼预测器

标量卡尔曼滤波是对掺杂有噪声的随机信号进行线性估计。但经常要对信号的未来值进行预测，特别是在控制系统中。根据预测提前时间的多少，把预测分成1步、2步、…、m步预测，通常把1步预测记作。预测的步数越多，误差越大。这里讨论1步预测问题。信号模型和观测模型同前：37第三十七页，共六十九页，2022年，8月28日根据前一节，有一步线性预测递推公式：

其中，a(k)和β(k)可以通过使均方预测误差最小来确定。预测的均方误差可表示为将预测方程代入该式，并求导，就会得到一组正交方程：38第三十八页，共六十九页，2022年，8月28日解之，得

a(k)=a-cβ(k)将其代入预测方程，有

进一步可求出：

其中，

由以上表达式可以看出，可根据均方预测误差Pε(k/k-1)计算β(k)，然后再给出Pε(k+1/k)的均方预测误差。39第三十九页，共六十九页，2022年，8月28日最优一步预测器40第四十页，共六十九页，2022年，8月28日最优一步预测及滤波器41第四十一页，共六十九页，2022年，8月28日四、向量卡尔曼滤波器1、信号向量和数据向量如果要求对q个信号进行同时估计，这q个信号在k时刻的采样值记作x1(k)、x2(k)、…、xq(k)。假设每个信号都是由一阶自回归过程产生的，即第α个信号在时刻k的采样值为:xα(k)=aαxα(k-1)+wα(k-1)α=1，2，…，q

每个wα过程都是白的，零均值的，与其它过程的采样是独立的。于是把q个信号与q个白噪声组成的q维向量分别表示成42第四十二页，共六十九页，2022年，8月28日显然，

X(k)=AX(k-1)+W(k-1)式中，X(k)，X(k-1)，W(k-1)都是q维向量，A是个q×q阶矩阵，即如果信号不满足一阶递归差分方程，而满足二阶递归差分方程，即x(k)=ax(k-1)+bx(k-2)+w(k-1)

43第四十三页，共六十九页，2022年，8月28日定义两个分量

x1(k)=x(k)x2(k)=x1(k-1)=x(k-1)于是，有

最后，有

X(k)=AX(k-1)+W(k-1)

结果把一个二阶差分方程变成了一个一阶二维向量方程，该方程用起来更简单方便。44第四十四页，共六十九页，2022年，8月28日

用R(k)表示k时刻的距离，R(k)表示k时刻的速度，U(k)表示k时刻的加速度，T表示采样周期，则写成一般形式：

其中，

.45第四十五页，共六十九页，2022年，8月28日写成向量形式：

最后,有

即可写成一阶向量的形式。在对信号向量进行估计的过程中，同时产生r个含有噪声的测量值，记作z1(k)，z2(k)，…,zr(k)。则得到一组观测方程:

46第四十六页，共六十九页，2022年，8月28日其中，vi(k)表示附加噪声，ci表示第i个测量参数，于是有Z(k)=CX(k)+V(k)

式中，Z(k)，V(k)是r维向量，X(k)是q维向量，C是r×q阶矩阵。对于r=q，有C即是观测矩阵。

47第四十七页，共六十九页，2022年，8月28日

2、向量问题的表示根据前面的讨论，我们完全可以把前面的信号模型动态方程和观测方程写成如下形式:采用标量运算和矩阵运算的等价关系，推广到多维情况：48第四十八页，共六十九页，2022年，8月28日据此，可以将观测噪声的方差变成协方差矩阵

对两个信号的情况，则有

同理，也可以把系统噪声的方差变成协方差矩阵，即

由于系统噪声采样互不相关，该协方差矩阵的非对角线元素的值均为零。单一信号均方误差也可变成协方差矩阵，

49第四十九页，共六十九页，2022年，8月28日

3、向量卡尔曼滤波器利用前面的概念，直接把标量卡尔曼滤波器公式变成向量卡尔曼滤波器公式：滤波器增益：

式中，

实际上，它是预测协方差。

误差协方差矩阵：

50第五十页，共六十九页，2022年，8月28日用K(k)代替了B(k)，因K(k)是通用符号，如图：向量卡尔曼滤波器结构

51第五十一页，共六十九页，2022年，8月28日增益矩阵K(k)的计算流程如图所示：

增益矩阵计算流程

52第五十二页，共六十九页，2022年，8月28日4、向量卡尔曼预测器根据相同的推导方法，可以获得卡尔曼预测器方程组。预测方程：预测增益:预测均方误差:

它们与标量的情况是一一对应的，只是用G(k)代替了β(k)。就可以将滤波和预测用同一个方框图表示出来。

53第五十三页，共六十九页，2022年，8月28日

5、总结卡尔曼滤波器应用广泛，这里只对其进行简单归纳。

1）卡尔曼滤波器的主要特性卡尔曼滤波器是一个递归、线性、无偏和方差最小的滤波器，如果过程噪声和观测噪声是正态高斯白噪声，则它保持最佳特性。54第五十四页，共六十九页，2022年，8月28日2）卡尔曼滤波器模型目标运动模型：

位置测量模型：

55第五十五页，共六十九页，2022年，8月28日状态方程:

X(t+T)=Φ(t)X(t)+W(t)Q(t)=E［W(t)W(t)T］

观测方程:Z(t)=HX(t)+V(t)

R(t)=E［V(t)V(t)T］

56第五十六页，共六十九页，2022年，8月28日3)卡尔曼滤波器方程组残差:

预测方程:

状态估计:

卡尔曼滤波器增益:

57第五十七页，共六十九页，2022年，8月28日预测协方差:

估计协方差:

58第五十八页，共六十九页，2022年，8月28日五、卡尔曼滤波器的应用

1.系统矩阵假定系统矩阵是四维矩阵，即距离、速度、方位角及其变化率,它们分别由R，，θ和表示，距离方向上的加速度和角度方向的加速度分别由ur(k)和uθ(k)表示。状态方程为59第五十九页，共六十九页，2022年，8月28日则系统方程为

用标准符号x1，x2

，x3,x4分别表示R，R,θ,θ。式中，A为系统矩阵，W(k)为噪声项。..60第六十页，共六十九页，2022年，8月28日

2.观测矩阵