卡尔曼滤波与组合导航原理

无人系统导航定位技术----卡尔曼滤波与组合导航技术主要学习内容•最优估计与卡尔曼滤波•组合导航基本原理和方法学习参考资料1.秦永元.卡尔曼滤波与组合导航原理.西北工业大学出版社2.付梦印等.Kalman滤波理论及其在导航系统中的应用3.王志贤编著.最优状态估计与系统辨识.西北工业大学出版社.卡尔曼鲁道夫·卡尔曼（RudolfEmilKalman），匈牙利裔美国数学家，1930年出生于匈牙利首都布达佩斯。1953年于麻省理工学院获得电机工程学士，翌年硕士学位。1957年于哥伦比亚大学获得博士学位。1964年至1971年任职斯坦福大学。1971年至1992年任佛罗里达大学数学系统理论中心（CenterforMathematicalSystemTheory）主任。1972起任瑞士苏黎世联邦理工学院数学系统理论中心主任直至退休。先居住于苏黎世和佛罗里达。2009年获美国国家科学奖章。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。当输入为带有高斯白噪声的信号时，使期望输出和实际输出之间的均方根误差达到最小的线性系统，这种滤波方法以它的发明者鲁道夫.E.卡尔曼的名字命名为卡尔曼滤波。1最优估计与卡尔曼滤波1.1最优估计的基本概念估计的概念待求系统状态)(tX)()()(tVtXhtZ测量得出的数据测量噪声)(ˆtXX(t)的估计是Z(t)的函数,若为线性函数,则)(ˆtX称作X(t)的线性估计)(ˆtX解算1最优估计与卡尔曼滤波1.1最优估计的基本概念预测和平滑tt0t1当t=t1时,)(ˆtX称为X(t)的估计；设在[t0，t1]时间段内量测为Z，待求状态为()Xt当tt1时,)(ˆtX称为X(t)的预测；Z计算ˆ()Xt1最优估计与卡尔曼滤波1.1最优估计的基本概念某一指标函数最小达到若以量测估计的偏差的平方和达到最小为指标ˆˆ()()minTZZZZ则所得估计为最小二乘估计！1最优估计与卡尔曼滤波1.1最优估计的基本概念最小二乘估计该方法由高斯(KarlGauss)在1795年测定行星轨道而提出的参数估计算法。该算法特点是简单，不必知道被估计量及量测值相关的任何统计信息。原理：误差平方和最小。iiiVXHZ随机量测噪声量测矩阵量测向量被估计向量1最优估计与卡尔曼滤波1.1最优估计的基本概念111VXHZ222VXHZrrrVXHZVHXZ1nnm1m1m指标函数：min)ˆ()ˆ()ˆ(XHZXHZXTJ0T)ˆ()ˆ(ˆXHZXHZXXXJZHHHXT1T)(ˆ最小二乘估计1最优估计与卡尔曼滤波1.1最优估计的基本概念优点：算法简单，不必知道量测误差的统计信息；局限性：（1）只能估计确定性的常值向量，无法估计随机向量的时间过程；（2）最优指标只保证了量测的估计均方误差之和最小，而并未确保被估计量的估计误差达到最佳，所以估计精度不高。最小二乘估计的特点：1最优估计与卡尔曼滤波1.1最优估计的基本概念原理：被估计量估计误差方差最小。设为随机向量，为的量测向量，即，求的估计就是根据解算出，显然是的函数，由于是随机误差，所以无法从的函数关系式中直接求取，而必须按统计意义的最优标准求取。最小方差估计min)]}(ˆ[)](ˆ{[)ˆ()(ˆ)(ˆTMVZXZXZX,ZXXZXXXEJXVXZZ)(XZXXˆXＺXˆVZXZ最小方差估计等于量测为某一具体实现条件下的条件均值：]/[)(ˆMVZXZXE定理1XZXXZ1最优估计与卡尔曼滤波1.1最优估计的基本概念最小方差估计最小方差估计是的无偏估计。][))(ˆ(MVXZXEE定理2X定理3若被估计向量和量测向量都服从正态分布，且ZXmZEmE][,][XXZZXCmmE]))([(],Cov[TZXZXZZZCmmE]))([(]Var[TZZZ1nX1mZ则的最小方差估计为：X)()(ˆ1MVzXZZCCmZmZXX1最优估计与卡尔曼滤波1.1最优估计的基本概念最小方差估计估计的均方误差为：ZXZXZXPZXXCCCC1MV)](ˆVar[VHXZ对于线性关系：XXVCXmXCVV][,)(,)(,0)(VarEVarE和XV互不相关，则:)()()(ˆ1TMVXVXXHmZHmZXCCXVXXXHCHHCHP1TT)(CCC1最优估计与卡尔曼滤波1.1最优估计的基本概念最小方差估计还可写成：)()()(ˆ11T11T1MVXmZHHHZXXVVXCCCC11T1)(HHCPVXC例：设为服从正态分布的随机量，均值为方差为，对用台仪器同时直接测量，测量误差都是服从正态分布的随机变量，均值为零，方差为，求的最小方差估计和估计的均方差。XXmXCXmXCX1最优估计与卡尔曼滤波1.1最优估计的基本概念最小方差估计根据题意，量测方程为：VHZXT21mZZZZT111HT21mVVVV根据公式有：mixiVXXmZmCmCmCm1MV1)(ˆXZXVXVXCmCCCP1最优估计与卡尔曼滤波1.1最优估计的基本概念极大验后估计设为随机向量，为的量测，为条件下的条件概率密度（亦称的验后概率密度）。如果估计值使下列指标满足则称为的极大验后估计。定理4如果和都服从正态分布，则的极大验后估计与最小方差估计相等。XXZ)/(zxpzZXX)(ˆMAZXmax)/()(ˆMAzXxzxp)(ˆMAZXXXZX1最优估计与卡尔曼滤波1.1最优估计的基本概念贝叶斯估计设为被估计量，是的量测量，是根据给出的对的估计，为估计误差，如果标量函数具有性质（1）当时，（2）当时，（3）则称为对被估计量的损失函数，也称代价函数，并称其期望值为的贝叶斯风险。使贝叶斯风险达到最小的估计称为贝叶斯估计，记为XZX)(ˆZXZX)(ˆ~ZXXX)](ˆ[)~(ZXXXLL12~~XX0)~()~(12XXLL0~X0)~(XL)~()~(XXLL)~(XL)(ˆZXX)]~([)ˆ(XXLEB)(ˆZX)(ˆZXB1最优估计与卡尔曼滤波1.1最优估计的基本概念极大似然估计设为被估计量，为的量测，为条件下的条件概率密度，称为的似然函数。使似然函数最大的估计量为最大似然估计，记为。XXZ)/(xzpxXZ)/(xzpX)(ˆMLZX1最优估计与卡尔曼滤波1.1最优估计的基本概念线性最小方差估计如果将估值规定为量测矢量的线性函数，即X式中A和b分别是（n×m）阶和n维的矩阵和矢量。这样的估计方法称为线性最小方差估计。可证明，这种估计只需要被估计值X和量测值Z的一、二阶统计特性，所以，它比最小方差估计较为实用。bAZXZ)()(1zXZXLmZCCmZXZZXZXZXCCCCP11最优估计与卡尔曼滤波1.1最优估计的基本概念各种最优估计的比较估计方法特点最小二乘法对象：常值向量或随机向量；好处：不需要统计信息，算法简单缺点：精度不高。最小方差估计是所有估计中的最佳。求取条件均值需求条件概率密度，难！极大验后、贝叶斯、极大似然除正态分布，计算概率密度十分困难。常用于故障检测和识别的算法中。线性最小方差估计所有线性估计中最优，只需求取被估计量和量测量的一阶和二阶矩。1最优估计与卡尔曼滤波1.2离散卡尔曼滤波卡尔曼滤波特点线性最小方差估计的问题：平稳过程——简单，因为其一阶、二阶矩皆为常值。非平稳过程---复杂，因为其一阶、二阶矩随时间变化，难以适用！1960年由卡尔曼（R.E.Kalman）首次提出，是一种线性最小方差估计，其特点：（1）算法是递推的，且使用状态空间法在时域内设计滤波器，所以卡尔曼滤波适用于对多维随机过程的估计。1最优估计与卡尔曼滤波1.2离散卡尔曼滤波在k时刻以前估值的基础上，根据k时刻的量测值Zk,递推得到k时刻的状态估计值：根据k-1时刻以前所有的量测值得到1kXkZkX)(ˆtXX（k）也可以说是综合利用k时刻以前的所有量测值得到的一次仅处理一个量测量计算量大大减小主要适用于线性动态系统！1最优估计与卡尔曼滤波1.2离散卡尔曼滤波离散卡尔曼滤波数学描述设离散化后的系统状态方程和量测方程分别为：kkkkkkkkkkVXHZWXX1111,Xk为k时刻的n维状态向量（被估计量）Zk为k时刻的m维量测向量k-1到k时刻的系统一步状态转移矩阵（n×n阶）Wk-1为k-1时刻的系统噪声（r维）Γk-1为系统噪声矩阵（n×r阶）Hk为k时刻系统量测矩阵（m×n阶）Vk为k时刻m维量测噪声1最优估计与卡尔曼滤波1.2离散卡尔曼滤波Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，在卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵；δkj是Kroneckerδ函数，即：)(1)(0jkjkkj卡尔曼滤波要求{Wk}和{Vk}是互不相关的零均值的白噪声序列，有：kjkTjkkjkTjkRVVEQWWE1最优估计与卡尔曼滤波1.2离散卡尔曼滤波Var{·}为对{·}求方差的符号卡尔曼滤波要求mx0和Cx0为已知量，初始状态的一、二阶统计特性为：00xmXE00xCXVar且要求X0与{Wk}和{Vk}都不相关1最优估计与卡尔曼滤波1.2离散卡尔曼滤波离散卡尔曼滤波方程1/)(kkkkkPHKIP或11,1/kkkkkXX状态一步预测方程)(1/1/kkkkkkkkXHZKXX状态估值计算方程11/1/)(kTkkkkTkkkkRHPHHPK滤波增益方程TkkkTkkkkkkkQPP1111,11,1/一步预测均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/估计均方差方程1最优估计与卡尔曼滤波1.2离散卡尔曼滤波离散卡尔曼滤波方程11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/时间修正方程量测修正方程1最优估计与卡尔曼滤波1.2离散卡尔曼滤波离散卡尔曼滤波方程物理意义（1）状态一步预测方程1ˆkXXk-1的卡尔曼滤波估值1/ˆkkX利用Xk-1计算得到的一步预测也可以认为是利用k-1时刻和以前时刻的量测值得到的Xk的一步预测1最优估计与卡尔曼滤波上式就是通过计算新息，把估计出来，并左乘一个系数矩阵加到中，从而得到估值和，称为滤波增益矩阵1/kkX1/~kkXkXˆkKkK（2）状态估值计算方程计算估值Xk的方程。它是在一步预测Xk/k-1的基础上，根据量测值Zk计算出来的)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/~一步预测误差1/1/~kkkkkXXX若把看作是量测的一步预测，则就是量测的一步预测误差1/kkkXH)(1/kkkkXHZkZ由两部分组成：和，正是在基础上估计所需信息，因此又称为新息1/kkXkV1/~kkX1/~kkXkX)(1/kkkkXHZ1最优估计与卡尔曼滤波（3）滤波增益方程Kk选取的标准就是卡尔曼滤波的估计准则，也就是使得均方误差阵最小：kXˆ由于也具有无偏性，即的均值为零，所以也称为一