1、无人系统导航定位技术无人系统导航定位技术 -卡尔曼滤波与组合导航技术卡尔曼滤波与组合导航技术 主要学习内容主要学习内容 最优估计与卡尔曼滤波最优估计与卡尔曼滤波 组合导航基本原理和方法组合导航基本原理和方法学习参考资料学习参考资料1.1.秦永元秦永元.卡尔曼滤波与组合导航原理卡尔曼滤波与组合导航原理.西北工业大学出版社西北工业大学出版社2.2.付梦印等付梦印等.Kalman.Kalman滤波理论及其在导航系统中的应用滤波理论及其在导航系统中的应用3.3.王志贤王志贤 编著编著.最优状态估计与系统辨识最优状态估计与系统辨识.西北工业大学出版社西北工业大学出版社.卡尔曼鲁道夫鲁道夫卡尔曼（卡尔曼（
2、Rudolf Emil KalmanRudolf Emil Kalman），匈），匈牙利裔美国数学家，牙利裔美国数学家，19301930年出生于匈牙利首都年出生于匈牙利首都布达佩斯。布达佩斯。19531953年于麻省理工学院获得电机工年于麻省理工学院获得电机工程学士，翌年硕士学位。程学士，翌年硕士学位。19571957年于哥伦比亚大年于哥伦比亚大学获得博士学位。学获得博士学位。19641964年至年至19711971年任职斯坦福年任职斯坦福大学。大学。19711971年至年至19921992年任佛罗里达大学数学系年任佛罗里达大学数学系统理论中心（统理论中心（Center for Mathema
3、tical Center for Mathematical System TheorySystem Theory）主任。）主任。19721972起任瑞士苏黎世联起任瑞士苏黎世联邦理工学院数学系统理论中心主任直至退休。邦理工学院数学系统理论中心主任直至退休。先居住于苏黎世和佛罗里达。先居住于苏黎世和佛罗里达。20092009年获美国国年获美国国家科学奖章。家科学奖章。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应
4、用坐标及速度。在很多工程应用(如雷达、计算机视觉如雷达、计算机视觉)中都可中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。当输入为带有高斯白噪声的信系统工程中的一个重要课题。当输入为带有高斯白噪声的信号时，使期望输出和实际输出之间的均方根误差达到最小的号时，使期望输出和实际输出之间的均方根误差达到最小的线性系统，这种滤波方法以它的发明者鲁道夫线性系统，这种滤波方法以它的发明者鲁道夫.E.E.卡尔曼的名卡尔曼的名字命名为卡尔曼滤波。字命名为卡尔曼滤波。1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1
5、 最优估计的基本概念最优估计的基本概念 估计的概念估计的概念 待求系统状态待求系统状态)(tX)()()(tVtXhtZ)(tX是是Z(t)Z(t)的函数的函数,若为线性函数若为线性函数,则则)(tX称作称作X(t)X(t)的线性估计的线性估计)(tX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念 预测和平滑预测和平滑 当当t=tt=t1 1时时,)(tX称为称为X(t)X(t)的估计；的估计；设在设在t t0 0，t t1 1 时间段内量测为时间段内量测为Z Z，待求状态为待求状态为()X t当当tttt1 1时时,)(tX称为称为X(t)X
6、(t)的预测；的预测；()X t1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念 若以量测估计的偏差的平方和达到最小为指标若以量测估计的偏差的平方和达到最小为指标()()minTZZZZ则所得估计为则所得估计为最小二乘估计最小二乘估计！1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 最小二乘估计最小二乘估计 该方法由高斯该方法由高斯(Karl Gauss)(Karl Gauss)在在17951795年测定行星轨道而提年测定行星轨道而提出的参数估计算法。该算法特点是简单，不必知道被估计量出的参数估
7、计算法。该算法特点是简单，不必知道被估计量及量测值相关的任何统计信息。及量测值相关的任何统计信息。原理：原理：误差平方和最小。误差平方和最小。iiiVXHZ随机量测噪声量测矩阵量测向量被估计向量1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念111VXHZ222VXHZrrrVXHZVHXZ1nnm1m1m指标函数：指标函数：min)()()(XHZXHZXTJ0T)()(XHZXHZXXXJZHHHXT1T)(l 最小二乘估计最小二乘估计 1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念优点：优点
8、：算法简单，不必知道量测误差的统计信息；算法简单，不必知道量测误差的统计信息；局限性：局限性：（1 1）只能估计确定性的常值向量，无法估计随机向量的）只能估计确定性的常值向量，无法估计随机向量的时间过程；时间过程；（2 2）最优指标只保证了量测的估计均方误差之和最小，）最优指标只保证了量测的估计均方误差之和最小，而并而并未确保被估计量的估计误差达到最佳，所以估计精度未确保被估计量的估计误差达到最佳，所以估计精度不高。不高。最小二乘估计的特点：最小二乘估计的特点：1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念原理：原理：被估计量估计误差方差最小。
9、被估计量估计误差方差最小。设设 为随机向量，为随机向量，为为 的量测向量，即的量测向量，即 ，求求 的估计的估计 就是根据就是根据 解算出解算出 ，显然，显然 是是 的函的函数，由于数，由于 是随机误差，所以是随机误差，所以 无法从无法从 的函数关系式中的函数关系式中直接求取，而必须按统计意义的最优标准求取。直接求取，而必须按统计意义的最优标准求取。l 最小方差估计最小方差估计 min)()()()()(TMVZXZXZX,ZXXZXXXEJXVXZZ)(XZXXXXVZXZ最小方差估计等于量测为某一具体实现条件下的条件均值：最小方差估计等于量测为某一具体实现条件下的条件均值：/)(MVZXZ
10、XE定理定理 1 1XZXXZ1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 最小方差估计最小方差估计 最小方差估计是最小方差估计是 的无偏估计。的无偏估计。)(MVXZXEE定理定理 2 2 X定理定理 3 3若被估计向量若被估计向量 和量测向量和量测向量 都服从正态分布，且都服从正态分布，且ZXmZEmE,XXZZXCmmE)(,CovTZXZXZZZCmmE)(VarTZZZ1nX1mZ则则 的最小方差估计为：的最小方差估计为：X)()(1MVzXZZCCmZmZXX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的
11、基本概念最优估计的基本概念l 最小方差估计最小方差估计 估计的均方误差为：估计的均方误差为：ZXZXZXPZXXCCCC1MV)(VarVHXZ对于线性关系：对于线性关系：XXVCXmXCVV,)(,)(,0)(VarEVarE和和XV互不相关，则互不相关，则:)()()(1TMVXVXXHmZHmZXCCXVXXXHCHHCHP1TT)(CCC1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 最小方差估计最小方差估计 还可写成：还可写成：)()()(11T11T1MVXmZHHHZXXVVXCCCC11T1)(HHCPVXC例：设例：设 为服
12、从正态分布的随机量，均值为为服从正态分布的随机量，均值为 方差为方差为 ，对，对 用用 台仪器同时直接测量，测量误差都是服台仪器同时直接测量，测量误差都是服从正态分布的随机变量，均值为零，方差为从正态分布的随机变量，均值为零，方差为 ，求，求 的最小方差估计和估计的均方差。的最小方差估计和估计的均方差。XXmXCXmXCX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 最小方差估计最小方差估计 根据题意，量测方程为：根据题意，量测方程为：VHZXT21mZZZZT111HT21mVVVV根据公式有：根据公式有：mixiVXXmZmCmCmCm
13、1MV1)(XZXVXVXCmCCCP1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 极大验后估计极大验后估计 设设 为随机向量，为随机向量，为为 的量测，的量测，为为 条条件下件下 的条件概率密度（亦称的条件概率密度（亦称 的验后概率密度）。如的验后概率密度）。如果估计值果估计值 使下列指标满足使下列指标满足则则 称为称为 的极大验后估计。的极大验后估计。定理定理 4 4 如果如果 和和 都服从正态分布，则都服从正态分布，则 的极大验后估计的极大验后估计与最小方差估计相等。与最小方差估计相等。XXZ)/(zxpzZXX)(MAZXmax)/
14、()(MAzXxzxp)(MAZXXXZX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 贝叶斯估计贝叶斯估计 设设 为被估计量，为被估计量，是是 的量测量，的量测量，是根据是根据 给给出的对出的对 的估计，的估计，为估计误差，如果标量函为估计误差，如果标量函数数具有性质具有性质 （1 1）当）当 时，时，（2 2）当）当 时，时，（3 3）则称则称 为为 对被估计量对被估计量 的损失函数，也称代价的损失函数，也称代价函数，并称其期望值函数，并称其期望值 为为 的贝叶斯风险。的贝叶斯风险。使贝叶斯风险达到最小的估计称为贝叶斯估计，记为使贝叶斯
15、风险达到最小的估计称为贝叶斯估计，记为 XZX)(ZXZX)(ZXXX)()(ZXXXLL12XX 0)()(12XXLL0X0)(XL)()(XXLL)(XL)(ZXX)()(XXLEB)(ZX)(ZXB1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 极大似然估计极大似然估计 设设 为被估计量，为被估计量，为为 的量测，的量测，为为 条条件下件下 的条件概率密度，的条件概率密度，称为称为 的似然函数。的似然函数。使似然函数最大的估计量为最大似然估计，记为使似然函数最大的估计量为最大似然估计，记为 。XXZ)/(xzpxX Z)/(xzpX)
16、(MLZX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 线性最小方差估计线性最小方差估计 n如果将估值如果将估值 规定为量测矢量规定为量测矢量 的线性函数，即的线性函数，即Xn式中式中A A 和和 b b 分别是（分别是（n nm m）阶和）阶和 n n 维的矩阵和矢量。维的矩阵和矢量。这这 样的估计方法称为样的估计方法称为线性最小方差估计线性最小方差估计。n可证明，这种估计只需要被估计值可证明，这种估计只需要被估计值X X和量测值和量测值Z Z 的一、的一、二阶统计特性，所以，它比最小方差估计较为实用。二阶统计特性，所以，它比最小方差估计
17、较为实用。bAZXZ)()(1zXZXLmZCCmZXZZXZXZXCCCCP11 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.1 1.1 最优估计的基本概念最优估计的基本概念l 各种最优估计的比较各种最优估计的比较1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 卡尔曼滤波特点卡尔曼滤波特点 线性最小方差估计的问题：线性最小方差估计的问题：平稳过程平稳过程简单，因为其一阶、二阶矩皆为常值。简单，因为其一阶、二阶矩皆为常值。非平稳过程非平稳过程-复杂，因为其一阶、二阶矩随时间变化，复杂，因为其一阶、二阶矩随时间变化，难以适用！难以适用！19601960
18、年由卡尔曼（年由卡尔曼（R.E.KalmanR.E.Kalman）首次提出，是一种线性）首次提出，是一种线性最小方差估计，其特点：最小方差估计，其特点：（1 1）算法是递推的，且使用状态空间法在时域内设计滤）算法是递推的，且使用状态空间法在时域内设计滤波器，所以卡尔曼滤波适用于对多维随机过程的估计。波器，所以卡尔曼滤波适用于对多维随机过程的估计。1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波n在在k k时刻以前估值的基础上，根据时刻以前估值的基础上，根据k k时刻的量测值时刻的量测值Z Zk k,递推得到递推得到k k时刻的状态估计值时刻的状态估计值
19、：根据根据k-1k-1时刻以前时刻以前所有的量测值得到所有的量测值得到 1kXkZkX)(tXX X（k k）也可以说是综合利用）也可以说是综合利用k k时刻以前的所有量测值得到时刻以前的所有量测值得到 的的一次仅处理一个量测量一次仅处理一个量测量计算量大大减小计算量大大减小1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 离散卡尔曼滤波数学描述离散卡尔曼滤波数学描述n设离散化后的系统状态方程和量测方程分别为：设离散化后的系统状态方程和量测方程分别为：kkkkkkkkkkVXHZWXX1111,X Xk k为为k k时刻的时刻的n n维状态向量维状态向
20、量（被估计量）（被估计量）Z Zk k为为k k时刻的时刻的m m维量测向量维量测向量k-1k-1到到k k时刻的系统一步状态时刻的系统一步状态转移矩阵（转移矩阵（n nn n阶）阶）WWk-1k-1为为k-1k-1时刻的系统噪声时刻的系统噪声（r r维）维）k-1k-1为系统噪声矩阵为系统噪声矩阵（n nr r阶）阶）H Hk k为为k k时刻系统量测矩阵时刻系统量测矩阵（m mn n阶）阶）V Vk k为为k k时刻时刻m m维量测噪声维量测噪声1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波nQ Qk k和和R Rk k分别称为系统噪声和量测噪声的
21、方差矩阵，分别称为系统噪声和量测噪声的方差矩阵，在卡尔曼滤波中要求它们分别是已知值的非负定阵在卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵；和正定阵；n k jk j是是Kronecker Kronecker 函数，即：函数，即：)(1)(0jkjkkjn卡尔曼滤波要求卡尔曼滤波要求WWk k 和和VVk k 是互不相关的零均值的是互不相关的零均值的白噪声序列，有：白噪声序列，有：kjkTjkkjkTjkRVVEQWWE1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波n Var Var 为对为对求方差的符号求方差的符号n卡尔曼滤波要求卡尔曼滤波要求
22、m mx0 x0和和C Cx0 x0为已知量为已知量，n初始状态的初始状态的 一、二阶统计特性为：一、二阶统计特性为：00 xmXE00 xCXVarn且要求且要求X X0 0与与WWk k 和和VVk k 都不相关都不相关1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 离散卡尔曼滤波方程离散卡尔曼滤波方程1/)(kkkkkPHKIP或 11,1/kkkkkXXn状态一步预测方程状态一步预测方程)(1/1/kkkkkkkkXHZKXXn状态估值计算方程状态估值计算方程11/1/)(kTkkkkTkkkkRHPHHPKn滤波增益方程滤波增益方程Tkkk
23、TkkkkkkkQPP1111,11,1/n一步预测均方差方程一步预测均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/n估计均方差方程估计均方差方程1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 离散卡尔曼滤波方程离散卡尔曼滤波方程11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/时间修正时间修正方程方程量测修正量测修正方程方程1 最优估计与卡尔曼滤波最优估计与
24、卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 离散卡尔曼滤波方程物理意义离散卡尔曼滤波方程物理意义（1 1）状态一步预测方程）状态一步预测方程1kXX Xk-1k-1的卡尔曼滤波估值的卡尔曼滤波估值1/kkX利用利用X Xk-1k-1计算得到的一步预测计算得到的一步预测 1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波 上式就是通过上式就是通过 计算新息，把计算新息，把 估计出来，并估计出来，并左乘一个系数矩阵左乘一个系数矩阵 加到加到 中，从而得到中，从而得到 估值估值 和，和，称为滤波增益矩阵称为滤波增益矩阵1/kkX1/kkXkXkKkK（2 2）状态估值计算方程）状态估值计算方
25、程)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/一步预测误差一步预测误差1/1/kkkkkXXX若把若把 看作是量测看作是量测 的一步预测，的一步预测，则则 就是量测的一步预测误差就是量测的一步预测误差1/kkkXH)(1/kkkkXHZkZ由两部分组成：由两部分组成：和和 ，正是在正是在 基础上估计基础上估计 所需信息，因此所需信息，因此又称又称 为新息为新息1/kkXkV1/kkX1/kkXkX)(1/kkkkXHZ1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波（3 3）滤波增益方程）滤波增益方程nK Kk k选取的标准就是卡尔曼滤波的
26、估计准则选取的标准就是卡尔曼滤波的估计准则，也就是使也就是使 得得 均方误差阵最小均方误差阵最小：kXn由于由于 也具有无偏性，即也具有无偏性，即 的均值为零，所的均值为零，所以以 也称为一步预测误差方差阵。上式中的也称为一步预测误差方差阵。上式中的 和和 分别就是新息中的两部分内容分别就是新息中的两部分内容1/kkX1/kkX1/kkPTkkkkHPH1/kR一步预测均方差阵，即：一步预测均方差阵，即：/1/1/1,Tkkk kk kk kZH PE XX11/1/)(kTkkkkTkkkkRHPHHPK如果如果R Rk k大，大，K Kk k就小就小R Rk k小，小，K Kk k就大就大
27、n由于由于 也具有无偏性，即也具有无偏性，即 的均值为零，所的均值为零，所以以 也称为一步预测误差方差阵。上式中的也称为一步预测误差方差阵。上式中的 和和 分别就是新息中的两部分内容分别就是新息中的两部分内容1/kkX1/kkX1/kkPTkkkkHPH1/kR一步预测均方差阵，即：一步预测均方差阵，即：/1/1/1,Tkkk kk kk kZH PE XX11/1/)(kTkkkkTkkkkRHPHHPK如果如果R Rk k大，大，K Kk k就小就小R Rk k小，小，K Kk k就大就大1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波（4 4）一步预测均方误差方程）一步预测均方误差方程11/
28、1/)(kTkkkkTkkkkRHPHHPKn从下式可以看出，求从下式可以看出，求K Kk k必须先求出必须先求出P Pk/k-1k/k-1n式中式中 ，为为 的估计误差，可以的估计误差，可以看出一步预测均方误差阵看出一步预测均方误差阵P Pk/k-1k/k-1是从估计均方误差阵是从估计均方误差阵P Pk-1k-1转移过来的，并且再加上系统噪声方差的影响。转移过来的，并且再加上系统噪声方差的影响。111kkkXXX1kXTkkkTkkkkkkkQPP1111,11,1/的均方误差阵，即：的均方误差阵，即：TkkkXXEP111,1kX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波（5 5）估计均
29、方误差方程）估计均方误差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/1/)(kkkkkPHKIP或 1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 离散卡尔曼滤波计算流程离散卡尔曼滤波计算流程1/kk11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/1kX1kkkX1kPkP1kk1,kkTkkkQ111kRkHkRkH1/kkPkKkZ1 最优估计与卡尔曼滤波最
30、优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 离散卡尔曼滤波初值确定离散卡尔曼滤波初值确定n在滤波开始时，必须有初始值在滤波开始时，必须有初始值 和和 才能进行才能进行0X0Pn为了保证估值的无偏性，应选择：为了保证估值的无偏性，应选择：000 xmXEXTxxTmXmXEXXXXEP)()(00000000000 xCXVarn这样才能保证估计均方差阵这样才能保证估计均方差阵P Pk k始终最小。始终最小。1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 动力学方程为连续微分方程时的离散化动力学方程为连续微分方程时的离散化kkkk
31、kkWXX,11)()()()()(twttttXGXF线性时变系统离散系统其中，系统的驱动源其中，系统的驱动源 为白噪声过程，即为白噪声过程，即)(tW)(tW0)(twE)()()()(ttwtwEq 为为 的方差强度阵的方差强度阵q)(twkjkTjkEQWW 为系统噪声方差阵为系统噪声方差阵kQ0)(twE)()()()(ttwtwEq 为为 的方差强度阵的方差强度阵q)(tw0)(twE)()()()(ttwtwEq 为为 的方差强度阵的方差强度阵q)(tw1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 动力学方程为连续微分方程时的离散化动
32、力学方程为连续微分方程时的离散化n根据连续系统的系统矩阵根据连续系统的系统矩阵 F(t)F(t)计算出离散系统的转移矩阵计算出离散系统的转移矩阵 K K/K-1/K-1n根据连续系统的系统噪声方差强度阵根据连续系统的系统噪声方差强度阵q q(t)(t)计算出离散计算出离散系统噪声方差阵系统噪声方差阵 Q Qk k1 1）K K/K-1/K-1 的计算的计算 根据线性系统理论，线性时变连续系统的解为：根据线性系统理论，线性时变连续系统的解为：dttttttt)()(),()(),()(wGXX0001 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 动力学
33、方程为连续微分方程时的离散化动力学方程为连续微分方程时的离散化dttttkkttkkkk)()(),(),()(wGX1111),()(),(kktttttFI),(tt),(),(),(020112tttttt 因此，其离散形式可以写成：因此，其离散形式可以写成：为状态转移矩阵，具有如下性质：为状态转移矩阵，具有如下性质：特别的：线性定常系特别的：线性定常系统，其状态转移矩阵为：统，其状态转移矩阵为：)(exp)(),(tttF1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 动力学方程为连续微分方程时的离散化动力学方程为连续微分方程时的离散化)(!
34、0)(/1knnnTtFkktFnTek式中：式中：为滤波器的计算周期为滤波器的计算周期 TkkttT13322/1!3!2kkTTTkkkFFFI 如果计算周期如果计算周期T T远小于系统阵远小于系统阵 F(t)F(t)发生明显变化所需要发生明显变化所需要的时间的时间，则则K K/K-1/K-1可以利用可以利用定常系统定常系统的计算方法，即的计算方法，即 1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 动力学方程为连续微分方程时的离散化动力学方程为连续微分方程时的离散化 另外，如果另外，如果F(t)F(t)在计算周期在计算周期T T内变化比较剧烈，
35、则将计内变化比较剧烈，则将计算周期分为算周期分为 个连续的子周期。在个连续的子周期。在 内每隔内每隔 就能得到系统矩阵的采样值：就能得到系统矩阵的采样值：,)2(,)1()1(,1kkkkkkkktTtTNtTNtTNtTNtN1,kkttNTt)()(titFiFkk1,2,1,0Ni 则一步转移矩阵则一步转移矩阵1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 动力学方程为连续微分方程时的离散化动力学方程为连续微分方程时的离散化 则：则：10,1)()0()2()1(NikkkkkkiFTTFNTFNTFIIII2 2）Q Qk k的计算的计算dt
36、qGGtEkTTkttkjTjkkkk),()()(),()(111WWQ1,kktt 在在 内，取内，取 并记：并记：)()(kttGG)()(kTkttqGGQ1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.2 1.2 离散卡尔曼滤波离散卡尔曼滤波l 动力学方程为连续微分方程时的离散化动力学方程为连续微分方程时的离散化)21()()21(10102NiTiNikkFiQQiFiTQTQ1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.3 1.3 连续系统卡尔曼滤波连续系统卡尔曼滤波)()()()()(twtGtXtFtX)()()()(tvtXtHtZ系统矩阵系统噪声矩阵系统噪声向量量测矩阵
37、噪声矩阵0)(twE)()()()(ttwtwEq 为为 的方差强度阵的方差强度阵)(tq)(tw0)(tE v)()()()(tttErvv)(tq)(tw 为为 的方差强度阵的方差强度阵)(tr)(tr)(tr)(tv1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.3 1.3 连续系统卡尔曼滤波连续系统卡尔曼滤波)()()()()()()(tXtHtZtKtXtFtX)()()()(1ttHtPtKTr)()()()()()()()()()()()()(1tGttGtPtHttHtPtPtFtFtPtPTTTqr连续系统卡尔曼滤波形式：连续系统卡尔曼滤波形式：黎卡蒂（黎卡蒂（Riccati
38、)Riccati)方程方程1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.4 1.4 连续连续-离散卡尔曼滤波离散卡尔曼滤波 实际被估计状态的系统经常是连续系统，而量测是间实际被估计状态的系统经常是连续系统，而量测是间隔时间的，这种被估计对象常称为连续隔时间的，这种被估计对象常称为连续离散系统。离散系统。则系统方程和量测方程分别：)()()()()(twtGtXtFtXkkkkVXHZ1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.4 1.4 连续连续-离散卡尔曼滤波离散卡尔曼滤波滤波方程：滤波方程：量测修正方程量测修正方程)()()(tXtFtX)()()()()()()()(tGttGtF
39、tPtPtFtPTTq)(1-ktXXkk)(1-ktPPkk11-kkkkXHZKXXkkkk111()kkkkTTkkkkkKPHH PHR11)()()(kkkkPHKIKRKHKIPHKIPkkTkkkTkkkkk时间修正方程时间修正方程 :可以采用微分方程的数可以采用微分方程的数值解法来求解，也可以值解法来求解，也可以用离散化的方法求解用离散化的方法求解1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波 一般的非线性系统（连续）和离散系统的方程可由以下一般的非线性系统（连续）和离散系统的方程可由以下形式描述形式描述:),(),()()
40、,(),()(tttXhtZtttXftXvw,1,11kVXhZkWXfXkkkkkk 如果如果 或或 ，或或 的概率分布是任意的，那么的概率分布是任意的，那么上述系统所描述的将是属于非常一般地随机非线性系统。上述系统所描述的将是属于非常一般地随机非线性系统。这类系统的最优估计问题的求解非常困难。这类系统的最优估计问题的求解非常困难。为了简化问题分析，必须对噪声的统计特性给以符合实为了简化问题分析，必须对噪声的统计特性给以符合实际又便于处理的假定。际又便于处理的假定。)(twkW)(tvkV1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波
41、这里研究的非线性最优估计问题的随机非线性系统的数这里研究的非线性最优估计问题的随机非线性系统的数学模型属于如下类型学模型属于如下类型:)(),()()()(),()(tttXhtZttGttXftXvwkkkkkkkVkXhZWkXfX,1,111 其中其中 或或 ，或或 的概率分布是彼此不相关的的概率分布是彼此不相关的零均值白噪声序列，且它们与初始状态零均值白噪声序列，且它们与初始状态 或或 也不相关。也不相关。)(twkW)(tvkV)(0tX0X 目前解决此类问题的主要方法是将非线系统线性化。目前解决此类问题的主要方法是将非线系统线性化。1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5
42、 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波 基本假设：基本假设：非线性微分方程的理论解一定存在，而且这非线性微分方程的理论解一定存在，而且这个理论解与实际解之间的差能够用一个线性微分方程表示，个理论解与实际解之间的差能够用一个线性微分方程表示，称为称为“线性干扰方程线性干扰方程”，“小偏差方程小偏差方程”，“摄动方程摄动方程”。1.5.1 1.5.1 围绕标称状态的线性化围绕标称状态的线性化 当系统噪声和量测噪声恒为当系统噪声和量测噪声恒为0 0时，上述系统模型的解称时，上述系统模型的解称为非线性方程的理论解，又称为非线性方程的理论解，又称“标称轨迹标称轨迹”或标称状态。或标称状态。通
43、常记为通常记为 或或 ，和，和 或或 ，则有，则有)(tnXknXknZ)(tnZ),()(),()(ttXhtZttXftXnnnn,1,1kXhZkXfXknkknnk1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波 非线性系统的真轨迹运动与标称轨迹运动的偏差为：非线性系统的真轨迹运动与标称轨迹运动的偏差为：)()()()()()(tZtZtZtXtXtXnnnkkknkkkZZZXXX 如果这些偏差足够小，那么，可以围绕标称状态把如果这些偏差足够小，那么，可以围绕标称状态把 和和 展开成泰勒展开成泰勒(Taylor)(Taylor)级数
44、，并且可取一次近似值。级数，并且可取一次近似值。)(tX)(tZ)()()(),(),()()()()()(),(),()()()()()()()()()(ttXtXttXhttXhtZttGtXtXttXfttXftXtxtxtxtxtxtxtxtxnnnnvwn连续系统线性化连续系统线性化1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波 则有：则有：)()()()()()()()()()()()()()()(tZtZttXtHtZtZtXtXttGtXtFtXtXnnnnnnvw)()()()()()()()()(ttXtHtZttGtX
45、tFtXnnvw或：或：)()()()()(),()(),()(),()(),()(),()(),()(),()(),()(),()(),()(ttnnnnnnttnnntxttftxttftxttftxttftxttftxttftxttftxttftxttfttttXXXXXXXXXXXXXXXfF212221212111 称为雅克比矩阵称为雅克比矩阵)(tnF)(tnH1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波)()()()()(),()(),()(),()(),()(),()(),()(),()(),()(),()(),()(tt
46、nmmmnnttnnntxtthtxtthtxtthtxtthtxtthtxtthtxtthtxtthtxtthttttXXXXXXXXXXXXXXXhH212221212111 推导离散系统线性化卡尔曼滤波方程有两条途径：推导离散系统线性化卡尔曼滤波方程有两条途径：1 1）先进非线性连续系统的离散化，再进行线性化；（麻烦）先进非线性连续系统的离散化，再进行线性化；（麻烦）2 2）先线性化，后离散化。先线性化，后离散化。（方便）（方便）1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波)()()()()()()()()(ttXtHtZttGtX
47、tFtXnnvw线性化线性化:离散化离散化:kknkkkknkkkVXHZXX111,W)()(,)(),()(),()(),()(),()(),()(),()(),()(),()(),()(1111112111111111212112111121111121111111111knkttknkknkkknkkknknkkkkkkkkknkkkkkkkkknnkktxttftxttftxttftxttftxttftxttftxttftxttftxttfTTtFXXXXXXXXXXX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波)()()()(
48、)(),()(),()(),()(),()(),()(),()(),()(),()(),(),(knkknkttknkknkkknkkknknkkkkkkkkknkkkkkkkkttkknktxtthtxtthtxtthtxtthtxtthtxtthtxtthtxtthtxtthtthXXXXXXXXXXXXXXXH21222121211111 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波基于标称轨迹线性化的状态量偏差离散型卡尔曼滤波方程：基于标称轨迹线性化的状态量偏差离散型卡尔曼滤波方程：11,1/knkkkkXX1T1111knkkknk
49、kkkQPP,/1T1T1)(/knkkknknkkkkRHPHHPK)(/11kknkkkkkkXHZKXXTkkkTkkkkkkkKRKHKIPHKIP)()(1/Tttkknnnkk),(111-XfXXXXXXXmCPn000000,kkknXXX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波EKF EKF 是为了解决按标称轨迹线性化存在的以下问题：是为了解决按标称轨迹线性化存在的以下问题：（1 1）标称解难解；）标称解难解；（2 2）真轨迹与标称解之间偏差不能确保其足够小。）真轨迹与标称解之间偏差不能确保其足够小。1.5.2 按最
50、优状态估计线性化的卡尔曼滤波方程按最优状态估计线性化的卡尔曼滤波方程 广义（推广、扩展）卡尔曼滤波（广义（推广、扩展）卡尔曼滤波（EKF,Extended Kalman filtering)为此，改用另一种近似方法，即采用围绕最优化状态估为此，改用另一种近似方法，即采用围绕最优化状态估计计 或或 的线性化方法，现定义真轨迹与标称轨迹的线性化方法，现定义真轨迹与标称轨迹间的偏差为：间的偏差为：kX)(tX1 最优估计与卡尔曼滤波最优估计与卡尔曼滤波1.5 1.5 非线性系统卡尔曼滤波非线性系统卡尔曼滤波EKF EKF 是为了解决按标称轨迹线性化存在的以下问题：是为了解决按标称轨迹线性化存在的以下
