卡尔曼滤波的学习

1.2Kalman滤波理论的基础在估计问题中，长考虑如下随机线性离散系统模型,11,11kkkkkkkXXWkkkkZHXVkX是系统的n维状态向量，kZ是系统的m维观察向量。根据状态向量和观察向量在时间上存在的不同对应关系，我们可以把估计问题分为滤波、预测和平滑，以上式所描述的随机线性离散系统为例，设,ˆkjX表示根据j时刻和j以前时刻的观察值，对k时刻状态kX做出的某种估计，则按照k和j的不同对应关系，叙述如下：（1）当k=j时，对,ˆkjX的估计称为滤波，即依据过去直至现在的观察测量来估计现在的状态。相应地，称,ˆkjX为kX的最有滤波估计值，简记为ˆkX。这类估计主要用于随机系统的实时控制。（2）当kj时对,ˆkjX的估计称为预测或外推，即依据过去直至现在的观察测量来预测未来的状态，并把,ˆkjX称为kX的最优预测估计值。这类估计广泛应用于对系统未来状态的预测和实时控制。（3）当kj时对,ˆkjX的估计称为平滑或内插，即依据过去直至现在的观察测量去估计过去的历史状态，并称,ˆkjX为kX的最优平滑估计值。这类估计广泛应用于通过分析实验或试验数据，对系统进行评估。在预测、滤波和平滑三类状态估计问题中预测是滤波的基础，滤波是平滑的基础。最早的估计方法是高斯提出的最小二乘法，最小二乘法没有考虑到被估参数和观测数据的统计特性，因此这种方法不是最优估计方法。Wiener滤波器采用频域设计法，运算复杂，解析求解困难，整批数据处理要求存储空间大，造成其适用范围及其有限，仅适用于一维平稳随机过程信号滤波。Kalman滤波采用了和Wiener滤波相同的估计准则，二者的基本原理一致，但是kalman滤波是一种时域滤波方法，采用状态空间方法描述系统，算法采用递推形式，数据存储量小，不仅可以处理平稳随机过程，也可以处理多维和非平稳随机过程。关于系统过程噪声和观测噪声的统计特性如下：0,0,0TkkjkkjTkkjkkjTkjEWEWWQEVEVVREWV如果被估计状态kX和对kX的观测量kZ满足上式约束，系统过程噪声kW和观测噪声kV满足上式的假设，系统过程噪声方差阵kQ非负定，系统观测噪声方差阵kR正定，k时刻的观测为kZ，则kX的估计ˆkX可按下述方程求解：状态一步预测：,1,11ˆkkkkkXX状态估计1,1ˆˆˆkkkkkkkXXKZHX滤波增益矩阵1,1,1TTkkkkkkkkkKPHHPHR一步预测误差方差阵,1,11,1,11,1TTkkkkkkkkkkkkPPQ估计误差方差阵,1TTkkkkkkkkkkPIKHPIKHKRK其中1TkkkkKPHR其中,1kkkkkPIKHP111,1TkkkkkkPPHRHKalman滤波算法的特点：（1）由于Kalman滤波算法将被估计的信号看作在白噪声作用下一个随机线性系统的输出，并且其输入输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳序列的滤波，而且特别适用于非平稳马尔科夫序列或高斯-马尔科夫序列的滤波，因此其应用范围是十分广泛的。（2）由于Kalman滤波的基本方程时间域内的递推形式，其计算过程是一个不断地“预测-修正”过程，在求解是不要求存储大量的数据，并且一旦观测到了新的数据，随时可以算的新的滤波值，因此这种滤波方法非常便于实时处理，计算机实现。（3）由于滤波器的增益矩阵于观测无关，因此它可离线算出，从而可以减少实时在线计算量；在求滤波器增益矩阵kK时要求一个矩阵的逆，既要计算1,1TkkkkkHPHR，它的阶数之取决于观测方程的维数m而m通常是最小的这样，上面的求逆运算是比较方便的；另外在求解滤波器增益的过程中随时可以算得滤波器的精度指标kP，其对角线上的元就是滤波误差向量各分量的方差。（4）增益矩阵kK与初始方差0P，系统噪声方差阵1kQ以及观测噪声方差阵kR之间具有如下关系：由基本滤波方程可见，当kR增大时，kK就变小，噪声变大滤波增益就应取小。如果0P变小，1kQ变小，因为0P小表示初始估计较好，1kQ变小表示系统噪声变小，于是增益矩阵应变小以便较小的修正。扩展kalman滤波在车辆GPS/dr组合定位系统中的应用GPS/DR组合系统状态方程的建立取组合定位系统的状态变量为[,,,,,]TeeennnXxvaxva,其中ex，nx分别为车辆东向和北向的位置分量；ev，nv分别为车辆东向和北向的速度分量；ea，na分别为车辆东向和北向的加速度分量。则得到组合定位系统连续的状态方程为：()()()XtAXtUWt式中，010000001000100000000010000001100000enaaA001001eneanaaUa，00()00enaaWteana分别为2(0,)ea2(0,)na的高斯白噪声；eana分别为车辆东向和北向机动加速度变化率的相关时间常数；eana分别为车辆东向和北向机动加速度分量的“当前”均值。设采样周期为T，将系统连续的状态方程离散化，得到系统离散的状态方程为,1,1kkkkkkkXXUW式4.79式中，()()()()()()[]TkekekeknknknkXxvaxva,1(,1)(,1)[,]kkekknkkdiag令1eeaa，1nnaa，则(,1)ekk，(,1)nkk为21(,1)1(1)01(1)00eeeaTeeaTekkeaTTTeee21(,1)1(1)01(1)00nnnaTnnaTnkknaTTTeee123456TkUuuuuuu其中，21110.5(1)eTeeeeuTTea21110.5(1)eTeeeeuTTea12(1)eTeeuTea3(1)eTeuea21140.5(1)nTnnnnuTTea15(1)nTnnuTea6(1)nTnuea式4.79就是所建立的GPS/DR组合定位系统的状态方程。GPS/DR组合系统观测方程的建立将GPS输出的东向位置信息obse北向位置信息obsn，角速率陀螺的输出以及里程计（或车速计）在一个采样周期内输出的距离s作为观测量，；里程计的刻度系数取为1.观测量和状态变量之间的关系如下1obseexv2obsnnxv122tan()eneennenvvavatvvv22enssTvv于是系统连续的观测方程为：122222eobsnobsneenensenxvexvnvavaZvvsTvv1v2v分别为GPS接收机输出的东向位置和北向位置的观测噪声，可近似为21(0,)21(0,)的高斯白噪声；为陀螺的漂移，近似为2(0,)的高斯白噪声；s为里程计的观测噪声，近似为2(0,)s的高斯白噪声。将观测方程离散化，得到系统离散的观测方程为kkkZhXV（4.86）式中，()()TkobskobskkkZens()()()()()()22()()22()()eknknkekeknkkeknkeknkxxvavahXvvTvv，1()2()()()kkkkskvvV从式（4.86）知，观测方程是非线性的。采用扩展Kalman滤波进行线性化，将khX在预测值,1ˆkkX处按泰勒级数展开并忽略二次以上的高次项，得,1,1ˆˆkkkkkkkkZhXHXXV（4.87）化简得,1,1ˆˆkkkkkkkkkZHXVhXHX（4.88）其中,1ˆ123456100000000100000000kkkkkXXkhXHhhhhXhh2(,1)(,1)(,1)(,1)(,1)(,1)(,1)1222(,1)(,1)ˆˆˆˆˆˆˆ2ˆˆnkkekkekknkkekknkknkknkkekkavvvaavhvv(,1)222(,1)(,1)ˆˆˆnkknkkekkvhvv2(,1)(,1)(,1)(,1)(,1)(,1)(,1)3222(,1)(,1)ˆˆˆˆˆˆˆ2ˆˆekkekkekknkknkkekknkknkkekkavvvaavhvv(,1)422(,1)(,1)ˆˆˆekknkkekkvhvv(,1)522(,1)(,1)ˆˆˆekknkkekkTvhvv(,1)622(,1)(,1)ˆˆˆnkkekknkkTvhvv式（4.88）就是所建立的GPS/DR系统线性离散的观测方程。根据扩展Kalman滤波递推方程和所建立的GPS/DR组合定位系统的状态方程式4.79和4.88式，可得系统的递推滤波方程如下,1,1ˆˆˆ[]kkkkkkkXXKZhX,1,111ˆˆkkkkkkXXU式4.901,1,1[]TTkkkkkkkkkKPHHPHR,1,11,11TkkkkkkkkPPQ,1[]kkkkkPIKHP递推方程中的,1kk、kU的表达式可从式（4.81）获得，kH可从式4.88获得，kR和系统的观测噪声的协方差有关，kQ如下22()()[][2,2]enTkkkaeekannkQEWWdiagaQaQ111213()212223313233eeeekeeeeeeqqqQqqqqqq，111213()212223313233nnnnknnnnnnqqqQqqqqqq其中2533122110.5(122324)eeTTeeeeeeqeTTTTe242212210.5(1222)eeeTTTeeeeeeqqeeTeTT2313310.5(12)eeTTeeeeqqeTe2223320.5(12)eeTTeeeqqee23220.5(342)eeTTeeeqeeT21330.5(1)eTeeqe式中()ekQ()nkQ都是对称矩阵，()nkQ中的元素表达式和()ekQ中的元素表达式相似，将()ekQ中的各元素表达式中的e用n来代替，即可相应地得到()nkQ中的元素表达式。若把加速度的一步预测看作“当前”加速度的均值，即()(,1)ˆekekkaa()(,1)ˆnknkkaa则式4.90可化简为,11(,1)1ˆˆkkkkkXX1(,1)11[(),()]kkendiagTT21112()()01001enTTTTT值得注意的是，在实际的使用中为了建立系统精确的数学模型，必须事先对系统GPS接收机的测量误差、陀螺漂移以及里程计的测量误差特性有深入的认识，这是一个相当复杂的过程且很重要的过程。否则，如果建立的系统模型和实际系统相差很大，则可能导致kalm