卡尔曼滤波与组合导航原理课件.ppt

《卡尔曼滤波与组合导航原理课件.ppt》由会员分享，可在线阅读，更多相关《卡尔曼滤波与组合导航原理课件.ppt（124页珍藏版）》请在三一办公上搜索。

1、无人系统导航定位技术 -卡尔曼滤波与组合导航技术,无人系统导航定位技术,主要学习内容,最优估计与卡尔曼滤波 组合导航基本原理和方法,主要学习内容,学习参考资料,1.秦永元.卡尔曼滤波与组合导航原理.西北工业大学出版社2.付梦印等.Kalman滤波理论及其在导航系统中的应用3.王志贤 编著.最优状态估计与系统辨识.西北工业大学出版社.,学习参考资料,卡尔曼,鲁道夫卡尔曼（Rudolf Emil Kalman），匈牙利裔美国数学家，1930年出生于匈牙利首都布达佩斯。 1953年于麻省理工学院获得电机工程学士，翌年硕士学位。1957年于哥伦比亚大学获得博士学位。1964年至1971年任职斯坦福大学

2、。1971年至1992年任佛罗里达大学数学系统理论中心（Center for Mathematical System Theory）主任。1972起任瑞士苏黎世联邦理工学院数学系统理论中心主任直至退休。先居住于苏黎世和佛罗里达。2009年获美国国家科学奖章。,卡尔曼鲁道夫卡尔曼（Rudolf Emil Kalman）,卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。当输入为带有高斯白噪声的信号时，使期望输出和

3、实际输出之间的均方根误差达到最小的线性系统，这种滤波方法以它的发明者鲁道夫.E.卡尔曼的名字命名为卡尔曼滤波。,卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念 估计的概念,待求系统状态,测量得出的数据,测量噪声,X(t)的估计,是Z(t)的函数,若为线性函数,则,称作X(t)的线性估计,解算,1 最优估计与卡尔曼滤波待求系统状态测量得出的数据测量,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念 预测和平滑,当t=t1时,称为X(t)的估计；,设在t0，t1 时间段内量测为Z，待求状态为,当tt1时,称为X(t)的预测；,

4、1 最优估计与卡尔曼滤波tt0t1当t=t1时,称为X,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,若以量测估计的偏差的平方和达到最小为指标,则所得估计为最小二乘估计！,1 最优估计与卡尔曼滤波某一指标函数最小达到若以量测估,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念 最小二乘估计,该方法由高斯(Karl Gauss)在1795年测定行星轨道而提出的参数估计算法。该算法特点是简单，不必知道被估计量及量测值相关的任何统计信息。 原理：误差平方和最小。,1 最优估计与卡尔曼滤波 该方法由高斯(Karl G,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,指标函数：,最小二

5、乘估计,1 最优估计与卡尔曼滤波指标函数： 最小二乘估计,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,优点：算法简单，不必知道量测误差的统计信息；局限性： （1）只能估计确定性的常值向量，无法估计随机向量的时间过程； （2）最优指标只保证了量测的估计均方误差之和最小，而并未确保被估计量的估计误差达到最佳，所以估计精度不高。,最小二乘估计的特点：,1 最优估计与卡尔曼滤波优点：算法简单，不必知道量测误,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,原理：被估计量估计误差方差最小。设 为随机向量， 为 的量测向量，即 ，求 的估计 就是根据 解算出 ，显然 是 的函数，由于 是随

6、机误差，所以 无法从 的函数关系式中直接求取，而必须按统计意义的最优标准求取。,最小方差估计,最小方差估计等于量测为某一具体实现条件下的条件均值：,定理 1,1 最优估计与卡尔曼滤波原理：被估计量估计误差方差最小,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,最小方差估计,最小方差估计是 的无偏估计。,定理 2,定理 3,若被估计向量 和量测向量 都服从正态分布，且,则 的最小方差估计为：,1 最优估计与卡尔曼滤波 最小方差估计 最小方差估计是,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,最小方差估计,估计的均方误差为：,对于线性关系：,和,互不相关，则:,1 最优估计与卡尔

7、曼滤波 最小方差估计 估计的均方误差,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,最小方差估计,还可写成：,例：设 为服从正态分布的随机量，均值为 方差为 ，对 用 台仪器同时直接测量，测量误差都是服从正态分布的随机变量，均值为零，方差为 ，求 的最小方差估计和估计的均方差。,1 最优估计与卡尔曼滤波 最小方差估计 还可写成：例：,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,最小方差估计,根据题意，量测方程为：,根据公式有：,1 最优估计与卡尔曼滤波 最小方差估计 根据题意，量测,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,极大验后估计,设 为随机向量， 为 的量

8、测， 为 条件下 的条件概率密度（亦称 的验后概率密度）。如果估计值 使下列指标满足则 称为 的极大验后估计。,定理 4,如果 和 都服从正态分布，则 的极大验后估计与最小方差估计相等。,1 最优估计与卡尔曼滤波 极大验后估计 设 为,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,贝叶斯估计,设 为被估计量， 是 的量测量， 是根据 给出的对 的估计， 为估计误差，如果标量函数具有性质 （1）当 时， （2）当 时， （3）则称 为 对被估计量 的损失函数，也称代价函数，并称其期望值 为 的贝叶斯风险。使贝叶斯风险达到最小的估计称为贝叶斯估计，记为,1 最优估计与卡尔曼滤波 贝叶斯估计

9、 设 为被,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,极大似然估计,设 为被估计量， 为 的量测， 为 条件下 的条件概率密度， 称为 的似然函数。使似然函数最大的估计量为最大似然估计，记为 。,1 最优估计与卡尔曼滤波 极大似然估计 设 为,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,线性最小方差估计,如果将估值 规定为量测矢量 的线性函数，即,式中A 和 b 分别是（nm）阶和 n 维的矩阵和矢量。这 样的估计方法称为线性最小方差估计。可证明，这种估计只需要被估计值X和量测值Z 的一、二阶统计特性，所以，它比最小方差估计较为实用。,1 最优估计与卡尔曼滤波 线性最小方

10、差估计 如果将估值,1 最优估计与卡尔曼滤波,1.1 最优估计的基本概念,各种最优估计的比较,1 最优估计与卡尔曼滤波 各种最优估计的比较,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,卡尔曼滤波特点,线性最小方差估计的问题： 平稳过程简单，因为其一阶、二阶矩皆为常值。 非平稳过程-复杂，因为其一阶、二阶矩随时间变化，难以适用！ 1960年由卡尔曼（R.E.Kalman）首次提出，是一种线性最小方差估计，其特点： （1）算法是递推的，且使用状态空间法在时域内设计滤波器，所以卡尔曼滤波适用于对多维随机过程的估计。,1 最优估计与卡尔曼滤波 卡尔曼滤波特点 线性最小方,1 最优估计与卡尔曼滤波

11、,1.2 离散卡尔曼滤波,在k时刻以前估值的基础上，根据k时刻的量测值Zk,递推得到k时刻的状态估计值 ：,根据k-1时刻以前所有的量测值得到,X（k）也可以说是综合利用k时刻以前的所有量测值得到 的,一次仅处理一个量测量计算量大大减小,主要适用于线性动态系统！,1 最优估计与卡尔曼滤波在k时刻以前估值的基础上，根据,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,离散卡尔曼滤波数学描述,设离散化后的系统状态方程和量测方程分别为：,Xk为k时刻的n维状态向量（被估计量）,Zk为k时刻的m维量测向量,k-1到k时刻的系统一步状态转移矩阵（nn阶）,Wk-1为k-1时刻的系统噪声（r维）,k-1

12、为系统噪声矩阵（nr阶）,Hk为k时刻系统量测矩阵（mn阶）,Vk为k时刻m维量测噪声,1 最优估计与卡尔曼滤波 离散卡尔曼滤波数学描述设离散,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，在卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵；,k j是Kronecker 函数，即：,卡尔曼滤波要求Wk和Vk是互不相关的零均值的白噪声序列，有：,1 最优估计与卡尔曼滤波Qk和Rk分别称为系统噪声和量,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,Var 为对求方差的符号,卡尔曼滤波要求mx0和Cx0为已知量，,初始状态的 一、二阶统计特性

13、为：,且要求X0与Wk和Vk都不相关,1 最优估计与卡尔曼滤波 Var 为对求方,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,离散卡尔曼滤波方程,或,1 最优估计与卡尔曼滤波 离散卡尔曼滤波方程或 状态一,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,离散卡尔曼滤波方程,时间修正方程,量测修正方程,1 最优估计与卡尔曼滤波 离散卡尔曼滤波方程时间修正量,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,离散卡尔曼滤波方程物理意义,（1）状态一步预测方程,Xk-1的卡尔曼滤波估值,利用Xk-1计算得到的一步预测,也可以认为是利用k-1时刻和以前时刻的量测值得到的Xk的一步预测,1 最优

14、估计与卡尔曼滤波 离散卡尔曼滤波方程物理意义（,1 最优估计与卡尔曼滤波,上式就是通过 计算新息，把 估计出来，并左乘一个系数矩阵 加到 中，从而得到 估值 和， 称为滤波增益矩阵,（2）状态估值计算方程,计算估值Xk的方程。它是在一步预测Xk/k-1的基础上，根据量测值Zk计算出来的,一步预测误差,若把 看作是量测 的一步预测，则 就是量测的一步预测误差,由两部分组成： 和 ， 正是在 基础上估计 所需信息，因此又称 为新息,1 最优估计与卡尔曼滤波 上式就是通过 计算新息，,1 最优估计与卡尔曼滤波,（3）滤波增益方程,Kk选取的标准就是卡尔曼滤波的估计准则，也就是使 得 均方误差阵最小：

15、,由于 也具有无偏性，即 的均值为零，所以 也称为一步预测误差方差阵。上式中的 和 分别就是新息中的两部分内容,一步预测均方差阵，即：,如果Rk大，Kk就小Rk小，Kk就大,由于 也具有无偏性，即 的均值为零，所以 也称为一步预测误差方差阵。上式中的 和 分别就是新息中的两部分内容,一步预测均方差阵，即：,如果Rk大，Kk就小Rk小，Kk就大,1 最优估计与卡尔曼滤波（3）滤波增益方程Kk选取的标,1 最优估计与卡尔曼滤波,（4）一步预测均方误差方程,从下式可以看出，求Kk必须先求出Pk/k-1,式中 ，为 的估计误差，可以看出一步预测均方误差阵Pk/k-1是从估计均方误差阵Pk-1转移过来的

16、，并且再加上系统噪声方差的影响。,的均方误差阵，即：,1 最优估计与卡尔曼滤波（4）一步预测均方误差方程从下,1 最优估计与卡尔曼滤波,（5）估计均方误差方程,或,计算量小，但在计算机有舍入误差的条件下，不能始终保证算出的Pk是对称的,1 最优估计与卡尔曼滤波（5）估计均方误差方程或 计算,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,离散卡尔曼滤波计算流程,滤波计算回路,增益计算回路,1 最优估计与卡尔曼滤波 离散卡尔曼滤波计算流程滤波计,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,离散卡尔曼滤波初值确定,在滤波开始时，必须有初始值 和 才能进行,为了保证估值的无偏性，应选择：,这

17、样才能保证估计均方差阵Pk始终最小。,1 最优估计与卡尔曼滤波 离散卡尔曼滤波初值确定在滤波,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,动力学方程为连续微分方程时的离散化,线性时变系统,离散系统,其中，系统的驱动源 为白噪声过程，即,为 的方差强度阵,为系统噪声方差阵,为 的方差强度阵,为 的方差强度阵,1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,动力学方程为连续微分方程时的离散化,根据连续系统的系统矩阵 F(t)计算出离散系统的转移矩阵 K/K-1,根据连续系统的系统噪声方差强度阵q(t)计算出离散系统噪声方差阵 Qk,1

18、） K/K-1 的计算,根据线性系统理论，线性时变连续系统的解为：,1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,动力学方程为连续微分方程时的离散化,因此，其离散形式可以写成：,为状态转移矩阵，具有如下性质：,特别的：线性定常系统，其状态转移矩阵为：,1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,动力学方程为连续微分方程时的离散化,式中： 为滤波器的计算周期,如果计算周期T远小于系统阵 F(t) 发生明显变化所需要的时间，则K/K-1可以利用定常系统的计算方法，即,1 最优估

19、计与卡尔曼滤波 动力学方程为连续微分方程时的,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,动力学方程为连续微分方程时的离散化,另外，如果F(t) 在计算周期T内变化比较剧烈，则将计算周期分为 个连续的子周期。在 内每隔 就能得到系统矩阵的采样值：,则一步转移矩阵,1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,动力学方程为连续微分方程时的离散化,则：,2）Qk的计算,在 内，取 并记：,1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的,1 最优估计与卡尔曼滤波,1.2 离散卡尔曼滤波,动力学方程为连续微分方程时的离散化,1

20、最优估计与卡尔曼滤波 动力学方程为连续微分方程时的,1 最优估计与卡尔曼滤波,1.3 连续系统卡尔曼滤波,系统矩阵,系统噪声矩阵,系统噪声向量,量测矩阵,噪声矩阵,为 的方差强度阵,为 的方差强度阵,1 最优估计与卡尔曼滤波系统矩阵系统噪声矩阵系统噪声向,1 最优估计与卡尔曼滤波,1.3 连续系统卡尔曼滤波,连续系统卡尔曼滤波形式：,黎卡蒂（Riccati)方程,1 最优估计与卡尔曼滤波连续系统卡尔曼滤波形式：,1 最优估计与卡尔曼滤波,1.4 连续-离散卡尔曼滤波,实际被估计状态的系统经常是连续系统，而量测是间隔时间的，这种被估计对象常称为连续离散系统。则系统方程和量测方程分别：,1 最优估

21、计与卡尔曼滤波 实际被估计状态的,1 最优估计与卡尔曼滤波,1.4 连续-离散卡尔曼滤波,滤波方程：,量测修正方程,时间修正方程 :,可以采用微分方程的数值解法来求解，也可以用离散化的方法求解,1 最优估计与卡尔曼滤波滤波方程：量测修正方程时间修正,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,一般的非线性系统（连续）和离散系统的方程可由以下形式描述:,或,如果 或 ， 或 的概率分布是任意的，那么上述系统所描述的将是属于非常一般地随机非线性系统。这类系统的最优估计问题的求解非常困难。 为了简化问题分析，必须对噪声的统计特性给以符合实际又便于处理的假定。,1 最优估计与卡尔曼滤波 一

22、般的非线性系统（连,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,这里研究的非线性最优估计问题的随机非线性系统的数学模型属于如下类型:,或,其中 或 ， 或 的概率分布是彼此不相关的零均值白噪声序列，且它们与初始状态 或 也不相关。,目前解决此类问题的主要方法是将非线系统线性化。,1 最优估计与卡尔曼滤波 这里研究的非,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,基本假设：非线性微分方程的理论解一定存在，而且这个理论解与实际解之间的差能够用一个线性微分方程表示，称为“线性干扰方程”，“小偏差方程”，“摄动方程”。,1.5.1 围绕标称状态的线性化 当系统噪声和量测噪声恒为

23、0时，上述系统模型的解称为非线性方程的理论解，又称“标称轨迹”或标称状态。通常记为 或 ，和 或 ，则有,1 最优估计与卡尔曼滤波 基本假设,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,非线性系统的真轨迹运动与标称轨迹运动的偏差为：,或,如果这些偏差足够小，那么，可以围绕标称状态把 和 展开成泰勒(Taylor)级数，并且可取一次近似值。,连续系统线性化,1 最优估计与卡尔曼滤波 非线性系统的真轨迹运动与,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,则有：,或：,称为雅克比矩阵,1 最优估计与卡尔曼滤波 则有：或： 称为雅克比,1 最优估计与卡尔曼滤波,1.5 非线性系

24、统卡尔曼滤波,推导离散系统线性化卡尔曼滤波方程有两条途径：1）先进非线性连续系统的离散化，再进行线性化；（麻烦）2）先线性化，后离散化。（方便）,1 最优估计与卡尔曼滤波 推导离散系统线性化卡尔曼,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,线性化:,离散化:,1 最优估计与卡尔曼滤波线性化:离散化:,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,1 最优估计与卡尔曼滤波,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,基于标称轨迹线性化的状态量偏差离散型卡尔曼滤波方程：,1 最优估计与卡尔曼滤波基于标称轨迹线性化的状态量偏差,1 最优估计与卡尔曼滤波,1.5 非

25、线性系统卡尔曼滤波,EKF 是为了解决按标称轨迹线性化存在的以下问题：（1）标称解难解；（2）真轨迹与标称解之间偏差不能确保其足够小。,1.5.2 按最优状态估计线性化的卡尔曼滤波方程 广义（推广、扩展）卡尔曼滤波（EKF, Extended Kalman filtering),为此，改用另一种近似方法，即采用围绕最优化状态估计 或 的线性化方法，现定义真轨迹与标称轨迹间的偏差为：,1 最优估计与卡尔曼滤波EKF 是为了解决按标称轨迹线,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,EKF 是为了解决按标称轨迹线性化存在的以下问题：（1）标称解难解；（2）真轨迹与标称解之间偏差不能确

26、保其足够小。,1.5.2 按最优状态估计线性化的卡尔曼滤波方程 广义（推广、扩展）卡尔曼滤波（EKF, Extended Kalman filtering),为此，改用另一种近似方法，即采用围绕最优化状态估计 或 的线性化方法，现定义真轨迹与标称轨迹间的偏差为：,1 最优估计与卡尔曼滤波EKF 是为了解决按标称轨迹线,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,或,或 为非线性化系统标称状态微分方程：,当初始值用初始状态最优估计 代入时的解。,或初始值用初始状态最优估计 对上式进行数值求解所得的解。,其中 就是系统状态 的一步预测值，即：,1 最优估计与卡尔曼滤波或 或,1 最优估

27、计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,离散型EKF方程： 采用先线性化后离散化的推导方法。,离散化,线性化,1 最优估计与卡尔曼滤波离散型EKF方程：离散化线性化,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,离散型EKF方程：,1 最优估计与卡尔曼滤波离散型EKF方程：,1 最优估计与卡尔曼滤波,1.5 非线性系统卡尔曼滤波,由于,因此：,所以上述离散卡尔曼滤波的最终形式为：,EKF为次优估计！,1 最优估计与卡尔曼滤波 由于因此：所以上述离散卡,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,例1：设运动体沿某一直线运动， 时刻的位移、速度、加速度和加加速度分别为

28、, , , ,只对运动体的位置作测量，测量值为 ,若： 量测量采样周期为T，求对 的估计。,1 最优估计与卡尔曼滤波例1：设运动体沿某一直线运动，,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,例：设电离层探测器上装有惯性导航系统，在飞行初始阶段用无线电定位测量的方法来实现飞行中导航参数的校正。为分析方便，仅考虑单轴情况，且认为惯导系统的误差源主要是初始条件（位置、速度、加速度）的误差。采用卡尔曼滤波信息融合的方法对惯导误差进行校正。,1 最优估计与卡尔曼滤波例：设电离层探测器上装有惯性导,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,1 最优估计与卡尔曼滤波,1 最优估计与卡

29、尔曼滤波,1.6 卡尔曼滤波应用举例,1 最优估计与卡尔曼滤波,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,1 最优估计与卡尔曼滤波,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,1 最优估计与卡尔曼滤波,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,1 最优估计与卡尔曼滤波,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,1 最优估计与卡尔曼滤波,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,例2：捷联惯导速度匹配对准,关键：建立好系统动力学方程和量测方程（a) 建立姿态估计误差和速度估计误差传播微分方程,（b) 用状态方程式表示,1 最优估计与卡尔曼

30、滤波例2：捷联惯导速度匹配对准关键,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,例3：捷联惯导速度匹配对准,离散形式：,1 最优估计与卡尔曼滤波例3：捷联惯导速度匹配对准离散,1 最优估计与卡尔曼滤波,1.6 卡尔曼滤波应用举例,例3：捷联惯导速度匹配对准,飞机导航系统的北向和东向速度测量值为：,导弹导航系统的北向和东向速度测量值估计值为：,以二者差值作为卡尔曼滤波器测量更新值：,量测方程为：,1 最优估计与卡尔曼滤波例3：捷联惯导速度匹配对准飞机,1 最优估计与卡尔曼滤波,1.7 卡尔曼滤波在融合导航系统中的应用方式,1、输出校正和反馈校正,（1）直接法估计和间接法估计,导航参数,

31、位置，L,速度VX ,VY ,VZ,姿态,1 最优估计与卡尔曼滤波1、输出校正和反馈校正 （1）,1 最优估计与卡尔曼滤波,1.7 卡尔曼滤波在融合导航系统中的应用方式,直接法,间接法,根据滤波器状态选取的不同，估计方法分为,以各种导航参数X为主要状态,滤波器估值的主要部分即是导航参数的估值,以某种导航系统输出导航参数的误差为主要状态,滤波器估值的主要部分即是导航参数误差的估值,模型可能是线性的，也可能是非线性的,模型一般都是线性的,根据间接法估计的状态都是误差状态，即滤波方程矢量是上述导航参数误差状态X和其他误差状态的集合（仍用X表示）,利用状态估值 去对原系统进行校正也有两种方法，即输出校

32、正和反馈校正两种。,1 最优估计与卡尔曼滤波直接法间接法根据滤波器状态选取,1 最优估计与卡尔曼滤波,1.7 卡尔曼滤波在融合导航系统中的应用方式,（2）输出校正,输出校正的滤波器,间接法的综合导航卡尔曼滤波器：将惯性系统和其他的导航系统各自计算的某些导航参数 （分别用XI和XN表示）进行比较，其差值就包含了惯导某些导航参数误差，即：,滤波器将这种差值作为量测值，经过滤波计算，得到滤波器的状态估值。,1 最优估计与卡尔曼滤波（2）输出校正 惯性系统卡尔曼,1 最优估计与卡尔曼滤波,1.7 卡尔曼滤波在融合导航系统中的应用方式,就是用导航参数误差的估值 去校正系统输出的导航参数，得到综合导航系统

33、的导航参数估值 （即经过校正后系统导航参数值）即：,定义 的估计误差 为：,即组合导航系统的导航参数 的误差 ，就是惯导系统导航参数误差估值 的估计误差 。,1 最优估计与卡尔曼滤波 就是用导航参数误差的估,1 最优估计与卡尔曼滤波,1.7 卡尔曼滤波在融合导航系统中的应用方式,（3）反馈校正,惯性系统,卡尔曼滤波器,其他导航系统,+,-,反馈校正的滤波器,采用反馈校正的间接法估计是将惯导系统导航参数误差 的估值 反馈到惯导系统内，对误差状态进行校正。,1 最优估计与卡尔曼滤波（3）反馈校正 惯性系统卡尔曼,1 最优估计与卡尔曼滤波,1.7 卡尔曼滤波在融合导航系统中的应用方式,输出校正与反馈

34、校正总结 从形式上看，输出校正仅仅校正系统得输出量，而反馈校正则是校正系统内部的状态。可以证明，两种校正方法的性质是一样的，具有同样的精度。 但是，输出校正的滤波器所估计的状态是未经校正的导航参数误差 ，而反馈校正的滤波器所估计的状态误差是经过校正的导航参数误差。前者数值大，后者数值小，而状态方程都是经过一阶近似的线性方程，状态的数值越小，则近似的准确性越高，因此，利用状态反馈校正的系统状态方程，更能接近真实地反映系统误差状态的动态过程。 故：对实际系统来讲，只要状态能够具体实施反馈校正，综合导航系统就可尽量采用反馈校正的滤波方案。,1 最优估计与卡尔曼滤波输出校正与反馈校正总结,2 组合导航

35、基本原理和方法,2.1 组合导航中的多传感器融合方法,集中式滤波,分散化滤波,利用一个卡尔曼滤波器来集中地处理所有导航子系统的信息,1971年Pearson提出分散滤波的概念利用子滤波器处理子系统的信息通过全局滤波器实现所有子系统的信息融合。1988年Carlson提出了联邦滤波器(Federated Filter),2 组合导航基本原理和方法 集中式滤波 分散化滤波,2 多传感器融合导航基本原理和方法,1）集中式滤波,2.1 组合导航中的多传感器融合方法,2 多传感器融合导航基本原理和方法 滤波器子系统1参考,2 多传感器融合导航基本原理和方法,集中式滤波器优点,(1) 可以给出最优的误差估

36、计；,(2) 只有一个滤波器，结构简单，工程中容易实现。,(3) 目前国内采用的仍然主要是集中式滤波。,2.2 组合导航中的多传感器融合方法,2 多传感器融合导航基本原理和方法 集中,2 组合导航基本原理和方法,集中式滤波器缺点：,集中式滤波理论上可以给出误差估计的最优估计，但存在着如下缺点：,(1) 状态维数高，计算负担重，不利于滤波实时运行，状态维数高会带来“维数灾难”；,(2) 容错性能差，不利于故障诊断，信息污染问题。,2.2 组合导航中的多传感器融合方法,2 组合导航基本原理和方法 集中式滤波器缺点：,2 组合导航基本原理和方法,2）分散化滤波,LF2,LF1,LFN,主滤波器,子系

37、统2,子系统1,子系统N,参考系统,时间更新,最优融合,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法 2）分散化滤波LF2LF1,2 组合导航基本原理和方法,2）分散化滤波,优点,(1) 全局滤波仍然较复杂；,(2) 算法基于各测量值是不相关的假设。,缺点,计算量小，容错性好。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法 2）分散化滤波优点,2 组合导航基本原理和方法,2）分散化滤波-一种简单的分散化滤波,假定系统的状态方程和量测方程为,其中 的协方差阵为 , 的协方差阵为,2.2 组合导航中的多传感器融合方法,2 组合导航基本原理和方法2）分散化滤波

38、-一种简单,2 组合导航基本原理和方法,2）分散化滤波-一种简单的分散化滤波,子系统的状态方程和量测方程为,其中 的协方差阵为 , 的协方差阵为,上式表示总系统利用了所有子系统的量测信息。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法2）分散化滤波-一种简单,2 多传感器融合导航基本原理和方法,2）分散化滤波-一种简单的分散化滤波,假设各子系统的量测相互独立，且 是 的一部分,2.1 组合导航中的多传感器融合方法,2 多传感器融合导航基本原理和方法2）分散化滤波-,2 组合导航基本原理和方法,2）分散化滤波-一种简单的分散化滤波,2.1组合导航中的多传感器融合方法,2 组合

39、导航基本原理和方法2）分散化滤波-一种简单,2 组合导航基本原理和方法,2）分散化滤波-一种简单的分散化滤波,全局滤波的量测更新可用局部滤波表示；时间更新仍需用全局滤波方程；子滤波和全局滤波都是最优的。,缺点：,全局滤波的合成算法比较复杂，不仅用到了子滤波器的滤波值和协方差，还用到了它们的预报值；算法基于各量测值是不相关的。,2.1组合导航中的多传感器融合方法,2 组合导航基本原理和方法2）分散化滤波-一种简单,2 组合导航基本原理和方法,2）分散化滤波-联邦滤波,2.2 组合导航中的多传感器融合方法,2 组合导航基本原理和方法2）分散化滤波-联邦滤波,2 组合导航基本原理和方法,2）分散化滤

40、波-联邦滤波,信息分配原则,（1）状态运动方程的信息状态方程的信息量是与系统噪声的方差成反比的，因此可用Q-1表示状态方程信息量；状态初值的信息也是状态方程的信息，可用初值估计的协方差阵的逆P-1(0)表示。,（2）量测方程的信息量测方程的信息可用量测噪声协方差的逆R-1表示。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法2）分散化滤波-联邦滤波,2 组合导航基本原理和方法,假设将系统噪声总的信息量Q-1分配到各局部滤波器和主滤波器中去,即,故:,根据信息守恒,状态估计初值P-1(0)也可按上述方法分配,可得,2.2 组合导航中的多传感器融合方法,2 组合导航基本原理和方法

41、 假设将系统噪声,2 组合导航基本原理和方法,（1）将子滤波器和主滤波器的初始估计协方差设置为 组合系统初始值的 倍,满足信息守恒原则；（2）将子滤波器和主滤波器的系统噪声协方差设置为 组合系统系统噪声协方差的 倍,满足信息守恒原则；（3）各子滤波器处理自己的量测信息,获得局部最优估计；（4）得到局部估计和主滤波器的估计后按下式最优合成：（5）用全局滤波解来重置各子滤波器和主滤波器的滤波值 和协方差阵。,2.2 组合导航中的多传感器融合方法,2 组合导航基本原理和方法（1）将子滤波器和主滤波器的,2 组合导航基本原理和方法,联邦滤波器的结构与性能分析,（1）第一类（ 零化式重置）,2.1 组合

42、导航中的多传感器融合方法,2 组合导航基本原理和方法联邦滤波器的结构与性能分析L,2 组合导航基本原理和方法,优、缺点主滤波器分配到全部信息，故障检测和隔离能力强;子滤波器状态信息只被重置到零，协方差趋于无穷，故障检测和隔离能力很差;减少了数据通讯量，计算简单。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法优、缺点,2 组合导航基本原理和方法,优、缺点主滤波器分配到全部信息，故障检测和隔离能力强;子滤波器状态信息只被重置到零，协方差趋于无穷，故障检测和隔离能力很差;减少了数据通讯量，计算简单。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法优、缺点,2 组

43、合导航基本原理和方法,LF2,LF1,LFN,主滤波器,子系统2,子系统1,子系统N,参考系统,时间更新,最优融合,（2）第二类（ 有重置）,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法LF2LF1LFN主滤波器子,2 组合导航基本原理和方法,优、缺点主滤波器与子滤波器之间平均分配信息；融合后全局滤波精度高，局部因为有全局滤波反馈，精度也提高了；子滤波器故障检测与隔离性能好；主滤波器的故障检测与隔离性能中等；一个子系统故障后，主滤波器受污染，隔离后必须重新初始化主滤波器。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法优、缺点,2 组合导航基本原理和方法,

44、（3）第三类（ 有重置）,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法LF2LF1LFN主滤波器子,2 组合导航基本原理和方法,优、缺点主滤波器状态方程无信息分配，主滤波器不需要进行滤波，所以主滤波器的估计值取为全局估计，即主滤波器的故障检测与隔离能力差；子滤波器的故障检测与隔离能力与第二类结构一样。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法优、缺点,2 组合导航基本原理和方法,（4）第四类（ 无重置）,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法（4）第四类（,2 组合导航基本原理和方法,优、缺点各局部滤波器独立滤波，没有反馈重

45、置带来的相互影响，提高了容错性能；由于没有全局估计，局部估计精度不高。,2.1 组合导航中的多传感器融合方法,2 组合导航基本原理和方法优、缺点,2 组合导航基本原理和方法,2.1 组合导航中的多传感器融合方法,（5）第五类（ 有重置）,2 组合导航基本原理和方法（5）第五类（,2 组合导航基本原理和方法,2.1 组合导航中的多传感器融合方法,优、缺点主滤波器包含所有状态信息；子滤波器的信息阵每次融合后重置为零；然后子滤波器再重新启动，即零化启动；子滤波器起到数据压缩作用，故障检测与隔离能力差；主滤波器的故障检测与隔离能力强，但是故障恢复能力差，因此要重新初始化。,2 组合导航基本原理和方法优

46、、缺点,2 组合导航基本原理和方法,2.1 组合导航中的多传感器融合方法,（6）第六类（ 有重置）,2 组合导航基本原理和方法LF1主滤波器子系统1子系统,2 组合导航基本原理和方法,2.1 组合导航中的多传感器融合方法,优、缺点每次信息融合后，子滤波器信息被重置到全局信息一半，协方差重置为全局估计协方差的一倍；主、子滤波器的故障检测与隔离能力中等；主滤波器的故障恢复能力差。是故障恢复能力差，因此要重新初始化。,2 组合导航基本原理和方法优、缺点,2 组合导航基本原理和方法,2.1 组合导航中的多传感器融合方法,结论：利用融合后的全局状态和协方差去反馈重置子滤波器，提高子滤波器精度，但是主滤波

47、器也因此容易受到故障子滤波器的影响；如果不将融合后的全局状态和协方差去反馈重置子滤波器，那么就不会产生交叉污染，容错性能大大提高。,2 组合导航基本原理和方法结论：,2 组合导航基本原理和方法,2.2 GNSS/INS 组合导航2.2.1 组合导航的基本体系结构,（1）松组合法,2 组合导航基本原理和方法（1）松组合法,2 组合导航基本原理和方法,2.2 GNSS/INS 组合导航,GNSS接收机通常通过自己的Kalman滤波输出其速度和位置，这种组合导致滤波器的串联，使组合导航观测噪声时间相关（有色噪声），不满足EKF观测噪声为白噪声的基本要求，严重时可能使滤波器不稳定。几乎无冗余信息，不利

48、于异常诊断，不利于进行随机模型改化 。,松组合的主要缺点,系统结构简单，易于实现，可以大幅度提高系统的导航精度，并使INS具有动基座对准能力。,松组合的主要优点,2 组合导航基本原理和方法GNSS接收机通常通过自己的,2 组合导航基本原理和方法,2.2 GNSS/INS 组合导航,（2）紧组合法,2 组合导航基本原理和方法（2）紧组合法,2 组合导航基本原理和方法,2.2 GNSS/INS 组合导航,观测量根据GNSS接收机收到的星历信息和INS输出的位置和速度信息，计算相应于INS位置的伪距和伪距率，GNSS接收机测量得到的伪距和伪距速率与INS计算值的差值。通过EKF对INS的误差和GPS

49、接收机的误差进行最优估计，然后对INS进行输出或者反馈校正。由于不存在滤波器的级联，并可对GNSS接收机的测距误差进行建模，因此这种伪距、伪距率组合方式比位置、速度组合具有更高的组合精度。而且在可见星的个数少于4颗时也可以使用。,2 组合导航基本原理和方法观测量根据GNSS接收机,2 组合导航基本原理和方法,2.2 GNSS/INS 组合导航,（3）深组合法,2 组合导航基本原理和方法（3）深组合法,2 组合导航基本原理和方法,2.2 GNSS/INS 组合导航,深组合是使用惯性导航信息对GNSS接收机进行辅助导航的组合方式。主要思想：既使用滤波技术对INS的误差进行最优估计，同时使用校正后的

50、INS速度信息对接收机的载波环、码环进行辅助跟踪，从而减小环路的等效带宽，增加GPS接收机在高动态或强干扰环境下的跟踪能力。嵌入式组合将INS和GNSS进行一体化设计，通过共用电源、时钟等进一步减小体积、降低成本和减小非同步误差的影响。,2 组合导航基本原理和方法 深组合是使用惯性导航信息对,2 组合导航基本原理和方法,2.3 组合导航方法举例,2 组合导航基本原理和方法,3 导航技术发展趋势,制导弹药等常规武器制导化领域：20世纪90年代中期，美国DRAPER实验室开始了面向常规弹药武器制导化组合导航系统发展趋势和需求的分析，研究基于MEMS惯性器件和抗干扰、防欺骗GPS接收机；研究一体化伪