卡尔曼滤波的学习

1.2 Kalman滤波理论的基础

在估计问题中，长考虑如下随机线性离散系统模型

Xk??k,k?1Xk?1??k,k?1Wk?1 Zk?HkXk?Vk

Xk是系统的n维状态向量，Zk是系统的m维观察向量。

根据状态向量和观察向量在时间上存在的不同对应关系，我们可以把估计问题分为滤波、预

?表示根据j时刻和j以前时刻的测和平滑，以上式所描述的随机线性离散系统为例，设Xk,j观察值，对k时刻状态Xk做出的某种估计，则按照k和j的不同对应关系， 叙述如下：

?的估计称为滤波，即依据过去直至现在的观察测量来估计现在的（1） 当k=j时，对Xk,j?。这类估计主要用于随?为X的最有滤波估计值，简记为X状态。相应地，称Xkkk,j机系统的实时控制。

?的估计称为预测或外推，即依据过去直至现在的观察测量来预测未（2） 当k>j时对Xk,j?称为X的最优预测估计值。这类估计广泛应用于对系统未来来的状态，并把Xkk,j状态的预测和实时控制。

?的估计称为平滑或内插，即依据过去直至现在的观察测量去估计过（3） 当k

在预测、滤波和平滑三类状态估计问题中预测是滤波的基础，滤波是平滑的基础。

最早的估计方法是高斯提出的最小二乘法，最小二乘法没有考虑到被估参数和观测数据的统计特性，因此这种方法不是最优估计方法。

Wiener滤波器采用频域设计法，运算复杂，解析求解困难，整批数据处理要求存储空间大，造成其适用范围及其有限，仅适用于一维平稳随机过程信号滤波。

Kalman滤波采用了和Wiener滤波相同的估计准则，二者的基本原理一致，但是kalman滤波是一种时域滤波方法，采用状态空间方法描述系统，算法采用递推形式，数据存储量小，不仅可以处理平稳随机过程，也可以处理多维和非平稳随机过程。

关于系统过程噪声和观测噪声的统计特性如下：

?E?Wk??0,E?WkWjT??Qk?kj????T?VV?E?Vk??0,E?kj???Rk?kj

?T?EWV??kj???0?如果被估计状态Xk和对Xk的观测量Zk满足上式约束，系统过程噪声Wk和观测噪声

Vk满足上式的假设，系统过程噪声方差阵Qk非负定，系统观测噪声方差阵Rk正定，k时刻

?可按下述方程求解： 的观测为Zk，则Xk的估计Xk状态一步预测：

? Xk,k?1??k,k?1Xk?1状态估计

??X??K?Z?HX??Xkk?1k?kkk,k?1?

滤波增益矩阵

TT?Kk?Pk,k?1HkHPH?Rk?kk,k?1k??

?1一步预测误差方差阵

TTP??P???Q?k,k?1k,k?1k?1k,k?1k,k?1k?1k,k?1

估计误差方差阵

TPk??I?KkHk?Pk,k?1?I?KkHk??KkRkKk

T其中

T?1Kk?PkHkRk

其中

Pk??I?KkHk?Pk,k?1

?1?1T?1Pk?Pk,k?1?HkRkHk

Kalman滤波算法的特点：

（1） 由于Kalman滤波算法将被估计的信号看作在白噪声作用下一个随机线性系

统的输出，并且其输入输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳序列的滤波，而且特别适用于非平稳马尔科夫序列或高斯-马尔科夫序列的滤波，因此其应用范围是十分广泛的。

（2） 由于Kalman滤波的基本方程时间域内的递推形式，其计算过程是一个不断

地“预测-修正”过程，在求解是不要求存储大量的数据，并且一旦观测到了新的数据，随时可以算的新的滤波值，因此这种滤波方法非常便于实时处理，计算机实现。

（3） 由于滤波器的增益矩阵于观测无关，因此它可离线算出，从而可以减少实时

在线计算量；在求滤波器增益矩阵Kk时要求一个矩阵的逆，既要计算

T?HPH?Rk?kk,k?1k??，它的阶数之取决于观测方程的维数m而m通常是最

?1小的这样，上面的求逆运算是比较方便的；另外在求解滤波器增益的过程中随时可以算得滤波器的精度指标P其对角线上的元就是滤波误差向量各分k，

量的方差。

（4）

增益矩阵Kk与初始方差P系统噪声方差阵Qk?1以及观测噪声方差阵Rk之0，间具有如下关系：由基本滤波方程可见，当Rk增大时，Kk就变小，噪声变大滤波增益就应取小。如果P0变小，Qk?1变小，因为P0小表示初始估计较好，Qk?1变小表示系统噪声变小，于是增益矩阵应变小以便较小的修正。

扩展kalman滤波在车辆GPS/dr组合定位系统中的应用

GPS/DR组合系统状态方程的建立

取组合定位系统的状态变量为X?[xe,ve,ae,xn,vn,an]T,其中xe，xn分别为车辆东向和北向的位置分量；ve，vn分别为车辆东向和北向的速度分量；ae，an分别为车辆东向和北向

?(t)?AX(t)?U?W(t) 的加速度分量。则得到组合定位系统连续的状态方程为：X式中，

?0?0???0A???0??0??0??en100?000011000000010000?a000e0??0??0??0?0?????0???1???0??ae???? U???ae?，W(t)???ae?

?0?0??0?????0?1??0??????11???an???an????an?????an??en22?a?a分别为(0,?a)(0,?an)的高斯白噪声；?a?a分别为车辆东向和北向机动加速度变e化率的相关时间常数；aean分别为车辆东向和北向机动加速度分量的“当前”均值。 设采样周期为T，将系统连续的状态方程离散化，得到系统离散的状态方程为

Xk??k,k?1Xk,k?1?Uk?Wk 式4.79

式中，Xk?[xe(k)ve(k)ae(k)xn(k)vn(k)an(k)]T

?k,k?1?diag[?e(k,k?1),?n(k,k?1)]

令ae?1?a，an?1e?a，则?e(k,k?1)，?n(k,k?1)为

n?e(k,k?1)?1T?e?2(?1??eT?e?aeT)?????01(1?e?aeT)?e?1?

?aeT?00?e???2?1T?n(?1??nT?e?anT)????1??01(1?e?anT)?n?

?anT?00?e???n(k,k?1)Uk??u1u2u3u4u5u6?

??eT?1)?e?1??ae e?T2?T?0.5?T?(1?e其中，u1??e???eT2?1?1u1????T?0.5?eT?(1?e)?e???eae ??eT?1?u2??T?(1?e)?e??ae

u3?(1?e??eT)ae

??nT2?1?1?u4??)?n??T?0.5?nT?(1?e??nan ??nT?1?u5??T?(1?e)?n??an

u6?(1?e??nT)an

式4.79就是所建立的GPS/DR组合定位系统的状态方程。

GPS/DR组合系统观测方程的建立

将GPS输出的东向位置信息eobs北向位置信息nobs，角速率陀螺的输出?以及里程计（或车速计）在一个采样周期内输出的距离s作为观测量，；里程计的刻度系数取为1.观测量和状态变量之间的关系如下

eobs?xe?v1 nobs?xn?v2

??vnae?vean???1ve?tan()?????? ???22?t?vn?ve?vn2s?Tve2?vn??s

于是系统连续的观测方程为：

xe???eobs????v1?xn?n????v?obsZ?????vnae?vean???2?

????22????????ve?vn????s????s?22??Tve?vn?可近似为(0,?12)(0,?12)的v1v2分别为GPS接收机输出的东向位置和北向位置的观测噪声，

2高斯白噪声；??为陀螺的漂移，近似为(0,??)的高斯白噪声；?s为里程计的观测噪声，近

似为(0,?s2)的高斯白噪声。

将观测方程离散化，得到系统离散的观测方程为

Zk?h?Xk??Vk （4.86）

式中，Zk???eobs(k)nobs(k)?ksk??

Txe(k)?????v1(k)?xn(k)???v?2(k)?h?Xk???vn(k)ae(k)?ve(k)an(k)?，Vk??

?????2?(k)ve2(k)?vn(k)?????????s(k)??22Tv?v?e(k)n(k)???从式（4.86）知，观测方程是非线性的。采用扩展Kalman滤波进行线性化，将h?Xk?在预

?测值Xk,k?1处按泰勒级数展开并忽略二次以上的高次项，得

?????Zk?h??Xk,k?1??Hk?Xk?Xk,k?1??Vk （4.87）

化简得

????HkXZk?HkXk?Vk?h?Xk,k?1k,k?1 （4.88） ??其中

Hk??h?Xk??Xk?Xk?Xk,k?1?10?00???0h1??0h500h2001000h30h620?0?? h4??0?

h1?2?n(k,k?1)v?e(k,k?1)?2v?e(k,k?1)v?n(k,k?1)a?e(k,k?1)?a?n(k,k?1)v?na(k,k?1)???v2n(k,k?1)??v2e(k,k?1)??

本文来源：https://www.bwwdw.com/article/njht.html