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

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

2、至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 最优估计的基本概念 预测和平滑 tt0t1当t=t1时,称为X(t)的估计；设在t0，t1 时间段内量测为Z，待求状态为当tt1时,称为X(t)的预测；Z计算1 最优估计与卡尔曼滤波tt

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

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

6、机误差，所以 无法从 的函数关系式中直接求取，而必须按统计意义的最优标准求取。 最小方差估计 最小方差估计等于量测为某一具体实现条件下的条件均值：定理 11 最优估计与卡尔曼滤波原理：被估计量估计误差方差最小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 最优估计与卡尔曼滤波 贝叶斯估计 设 为被1 最优估计与卡尔曼滤波1.1 最优估计的基

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

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

11、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为系统噪声矩阵（nr阶）Hk为k时刻系统量测矩阵（mn阶）Vk为k时刻m维量测噪声1 最优估计与卡尔曼滤

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

13、卡尔曼滤波方程或 状态一步预测方程状态估值计算方程滤波增益方程一步预测均方差方程估计均方差方程1 最优估计与卡尔曼滤波 离散卡尔曼滤波方程或 状态一1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 离散卡尔曼滤波方程时间修正方程量测修正方程1 最优估计与卡尔曼滤波 离散卡尔曼滤波方程时间修正量1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 离散卡尔曼滤波方程物理意义（1）状态一步预测方程Xk-1的卡尔曼滤波估值利用Xk-1计算得到的一步预测 也可以认为是利用k-1时刻和以前时刻的量测值得到的Xk的一步预测 1 最优估计与卡尔曼滤波 离散卡尔曼滤波方程物理意义（1 最优估计与卡尔曼滤波 上式就是通

14、过 计算新息，把 估计出来，并左乘一个系数矩阵 加到 中，从而得到 估值 和， 称为滤波增益矩阵（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转移过来的，并且再加上系统噪声方差的影响。 的均方误差阵，即：1 最优估计与卡尔曼滤波（4）一步预测均方误差方程从下1 最优估计与卡

16、尔曼滤波（5）估计均方误差方程或 计算量小，但在计算机有舍入误差的条件下，不能始终保证算出的Pk是对称的 1 最优估计与卡尔曼滤波（5）估计均方误差方程或 计算1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 离散卡尔曼滤波计算流程滤波计算回路增益计算回路1 最优估计与卡尔曼滤波 离散卡尔曼滤波计算流程滤波计1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 离散卡尔曼滤波初值确定在滤波开始时，必须有初始值 和 才能进行为了保证估值的无偏性，应选择：这样才能保证估计均方差阵Pk始终最小。1 最优估计与卡尔曼滤波 离散卡尔曼滤波初值确定在滤波1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 动力学方程为

17、连续微分方程时的离散化线性时变系统离散系统其中，系统的驱动源 为白噪声过程，即 为 的方差强度阵 为系统噪声方差阵 为 的方差强度阵 为 的方差强度阵1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 动力学方程为连续微分方程时的离散化根据连续系统的系统矩阵 F(t)计算出离散系统的转移矩阵 K/K-1根据连续系统的系统噪声方差强度阵q(t)计算出离散系统噪声方差阵 Qk1） K/K-1 的计算 根据线性系统理论，线性时变连续系统的解为：1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 动力学

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

19、分为 个连续的子周期。在 内每隔 就能得到系统矩阵的采样值： 则一步转移矩阵1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 动力学方程为连续微分方程时的离散化 则：2）Qk的计算 在 内，取 并记：1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的1 最优估计与卡尔曼滤波1.2 离散卡尔曼滤波 动力学方程为连续微分方程时的离散化1 最优估计与卡尔曼滤波 动力学方程为连续微分方程时的1 最优估计与卡尔曼滤波1.3 连续系统卡尔曼滤波系统矩阵系统噪声矩阵系统噪声向量量测矩阵噪声矩阵 为 的方差强度阵 为 的方差强度阵1 最优估计与卡尔曼滤波

20、系统矩阵系统噪声矩阵系统噪声向1 最优估计与卡尔曼滤波1.3 连续系统卡尔曼滤波连续系统卡尔曼滤波形式： 黎卡蒂（Riccati)方程1 最优估计与卡尔曼滤波连续系统卡尔曼滤波形式： 1 最优估计与卡尔曼滤波1.4 连续-离散卡尔曼滤波 实际被估计状态的系统经常是连续系统，而量测是间隔时间的，这种被估计对象常称为连续离散系统。则系统方程和量测方程分别： 1 最优估计与卡尔曼滤波 实际被估计状态的1 最优估计与卡尔曼滤波1.4 连续-离散卡尔曼滤波滤波方程：量测修正方程时间修正方程 :可以采用微分方程的数值解法来求解，也可以用离散化的方法求解1 最优估计与卡尔曼滤波滤波方程：量测修正方程时间修正

21、1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波 一般的非线性系统（连续）和离散系统的方程可由以下形式描述:或 如果 或 ， 或 的概率分布是任意的，那么上述系统所描述的将是属于非常一般地随机非线性系统。这类系统的最优估计问题的求解非常困难。 为了简化问题分析，必须对噪声的统计特性给以符合实际又便于处理的假定。 1 最优估计与卡尔曼滤波 一般的非线性系统（连1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波 这里研究的非线性最优估计问题的随机非线性系统的数学模型属于如下类型:或 其中 或 ， 或 的概率分布是彼此不相关的零均值白噪声序列，且它们与初始状态 或 也不相关。 目前解决此类问题

22、的主要方法是将非线系统线性化。1 最优估计与卡尔曼滤波 这里研究的非1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波 基本假设：非线性微分方程的理论解一定存在，而且这个理论解与实际解之间的差能够用一个线性微分方程表示，称为“线性干扰方程”，“小偏差方程”，“摄动方程”。1.5.1 围绕标称状态的线性化 当系统噪声和量测噪声恒为0时，上述系统模型的解称为非线性方程的理论解，又称“标称轨迹”或标称状态。通常记为 或 ，和 或 ，则有1 最优估计与卡尔曼滤波 基本假设1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波 非线性系统的真轨迹运动与标称轨迹运动的偏差为：或 如果这些偏差足够小，那么，

23、可以围绕标称状态把 和 展开成泰勒(Taylor)级数，并且可取一次近似值。连续系统线性化1 最优估计与卡尔曼滤波 非线性系统的真轨迹运动与1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波 则有：或： 称为雅克比矩阵1 最优估计与卡尔曼滤波 则有：或： 称为雅克比1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波 推导离散系统线性化卡尔曼滤波方程有两条途径：1）先进非线性连续系统的离散化，再进行线性化；（麻烦）2）先线性化，后离散化。（方便）1 最优估计与卡尔曼滤波 推导离散系统线性化卡尔曼1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波线性化:离散化:1 最优估计与卡尔曼滤波线性化

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

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

26、尔曼滤波1.5 非线性系统卡尔曼滤波或 或 为非线性化系统标称状态微分方程： 当初始值用初始状态最优估计 代入时的解。 或初始值用初始状态最优估计 对上式进行数值求解所得的解。 其中 就是系统状态 的一步预测值，即：1 最优估计与卡尔曼滤波或 或 1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波离散型EKF方程： 采用先线性化后离散化的推导方法。离散化线性化1 最优估计与卡尔曼滤波离散型EKF方程：离散化线性化1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波离散型EKF方程： 1 最优估计与卡尔曼滤波离散型EKF方程：1 最优估计与卡尔曼滤波1.5 非线性系统卡尔曼滤波 由于因此：所以

27、上述离散卡尔曼滤波的最终形式为：EKF为次优估计！1 最优估计与卡尔曼滤波 由于因此：所以上述离散卡1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例例1：设运动体沿某一直线运动， 时刻的位移、速度、加速度和加加速度分别为 , , , ,只对运动体的位置作测量，测量值为 ,若： 量测量采样周期为T，求对 的估计。 1 最优估计与卡尔曼滤波例1：设运动体沿某一直线运动，1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例例：设电离层探测器上装有惯性导航系统，在飞行初始阶段用无线电定位测量的方法来实现飞行中导航参数的校正。为分析方便，仅考虑单轴情况，且认为惯导系统的误差源主要是初始条件（位置、速度、

28、加速度）的误差。采用卡尔曼滤波信息融合的方法对惯导误差进行校正。1 最优估计与卡尔曼滤波例：设电离层探测器上装有惯性导1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例1 最优估计与卡尔曼滤波1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例1 最优估计与卡尔曼滤波1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例1 最优估计与卡尔曼滤波1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例1 最优估计与卡尔曼滤波1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例1 最优估计与卡尔曼滤波1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例1 最优估计与卡尔曼滤波1 最优估计与卡尔曼滤波1.6 卡尔曼

29、滤波应用举例例2：捷联惯导速度匹配对准关键：建立好系统动力学方程和量测方程（a) 建立姿态估计误差和速度估计误差传播微分方程（b) 用状态方程式表示1 最优估计与卡尔曼滤波例2：捷联惯导速度匹配对准关键1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例例3：捷联惯导速度匹配对准离散形式：1 最优估计与卡尔曼滤波例3：捷联惯导速度匹配对准离散1 最优估计与卡尔曼滤波1.6 卡尔曼滤波应用举例例3：捷联惯导速度匹配对准飞机导航系统的北向和东向速度测量值为：导弹导航系统的北向和东向速度测量值估计值为：以二者差值作为卡尔曼滤波器测量更新值：量测方程为：1 最优估计与卡尔曼滤波例3：捷联惯导速度匹配对准

30、飞机1 最优估计与卡尔曼滤波1.7 卡尔曼滤波在融合导航系统中的应用方式1、输出校正和反馈校正 （1）直接法估计和间接法估计 组合导航系统采用卡尔曼滤波进行估计的主要对象导航参数 导航参数 位置，L 速度VX ,VY ,VZ姿态,导航参数用X表示1 最优估计与卡尔曼滤波1、输出校正和反馈校正 （1）1 最优估计与卡尔曼滤波1.7 卡尔曼滤波在融合导航系统中的应用方式直接法间接法根据滤波器状态选取的不同，估计方法分为 以各种导航参数X为主要状态滤波器估值的主要部分即是导航参数的估值以某种导航系统输出导航参数的误差为主要状态滤波器估值的主要部分即是导航参数误差的估值模型可能是线性的，也可能是非线性

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

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

33、 反馈到惯导系统内，对误差状态进行校正。1 最优估计与卡尔曼滤波（3）反馈校正 惯性系统卡尔曼1 最优估计与卡尔曼滤波1.7 卡尔曼滤波在融合导航系统中的应用方式输出校正与反馈校正总结 从形式上看，输出校正仅仅校正系统得输出量，而反馈校正则是校正系统内部的状态。可以证明，两种校正方法的性质是一样的，具有同样的精度。 但是，输出校正的滤波器所估计的状态是未经校正的导航参数误差 ，而反馈校正的滤波器所估计的状态误差是经过校正的导航参数误差。前者数值大，后者数值小，而状态方程都是经过一阶近似的线性方程，状态的数值越小，则近似的准确性越高，因此，利用状态反馈校正的系统状态方程，更能接近真实地反映系统误

34、差状态的动态过程。 故：对实际系统来讲，只要状态能够具体实施反馈校正，综合导航系统就可尽量采用反馈校正的滤波方案。1 最优估计与卡尔曼滤波输出校正与反馈校正总结2 组合导航基本原理和方法2.1 组合导航中的多传感器融合方法 集中式滤波 分散化滤波 利用一个卡尔曼滤波器来集中地处理所有导航子系统的信息 1971年Pearson提出分散滤波的概念利用子滤波器处理子系统的信息通过全局滤波器实现所有子系统的信息融合。1988年Carlson提出了联邦滤波器(Federated Filter)2 组合导航基本原理和方法 集中式滤波 分散化滤波 2 多传感器融合导航基本原理和方法 滤波器子系统1参考系统时

35、间更新最优融合子系统2子系统21）集中式滤波 2.1 组合导航中的多传感器融合方法2 多传感器融合导航基本原理和方法 滤波器子系统1参考2 多传感器融合导航基本原理和方法 集中式滤波器优点(1) 可以给出最优的误差估计； (2) 只有一个滤波器，结构简单，工程中容易实现。 (3) 目前国内采用的仍然主要是集中式滤波。 2.2 组合导航中的多传感器融合方法2 多传感器融合导航基本原理和方法 集中2 组合导航基本原理和方法 集中式滤波器缺点： 集中式滤波理论上可以给出误差估计的最优估计，但存在着如下缺点： (1) 状态维数高，计算负担重，不利于滤波实时运行，状态维数高会带来“维数灾难”； (2)

36、容错性能差，不利于故障诊断，信息污染问题。 2.2 组合导航中的多传感器融合方法2 组合导航基本原理和方法 集中式滤波器缺点：2 组合导航基本原理和方法 2）分散化滤波LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法 2）分散化滤波LF2LF12 组合导航基本原理和方法 2）分散化滤波优点 (1) 全局滤波仍然较复杂；(2) 算法基于各测量值是不相关的假设。 缺点计算量小，容错性好。2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法 2）分散化滤波优点 2 组合导航基本原理和方法2）分散化滤波-

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

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

39、F1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合2.2 组合导航中的多传感器融合方法2 组合导航基本原理和方法2）分散化滤波-联邦滤波2 组合导航基本原理和方法2）分散化滤波-联邦滤波信息分配原则（1）状态运动方程的信息状态方程的信息量是与系统噪声的方差成反比的，因此可用Q-1表示状态方程信息量；状态初值的信息也是状态方程的信息，可用初值估计的协方差阵的逆P-1(0)表示。（2）量测方程的信息量测方程的信息可用量测噪声协方差的逆R-1表示。2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法2）分散化滤波-联邦滤波2 组合导航基本原理和方法 假设将系统噪声总的信息量

40、Q-1分配到各局部滤波器和主滤波器中去,即 故:根据信息守恒状态估计初值P-1(0)也可按上述方法分配,可得2.2 组合导航中的多传感器融合方法2 组合导航基本原理和方法 假设将系统噪声2 组合导航基本原理和方法（1）将子滤波器和主滤波器的初始估计协方差设置为 组合系统初始值的 倍,满足信息守恒原则；（2）将子滤波器和主滤波器的系统噪声协方差设置为 组合系统系统噪声协方差的 倍,满足信息守恒原则；（3）各子滤波器处理自己的量测信息,获得局部最优估计；（4）得到局部估计和主滤波器的估计后按下式最优合成：（5）用全局滤波解来重置各子滤波器和主滤波器的滤波值 和协方差阵。2.2 组合导航中的多传感器

41、融合方法2 组合导航基本原理和方法（1）将子滤波器和主滤波器的2 组合导航基本原理和方法联邦滤波器的结构与性能分析LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合（1）第一类（ 零化式重置）2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法联邦滤波器的结构与性能分析L2 组合导航基本原理和方法优、缺点主滤波器分配到全部信息，故障检测和隔离能力强;子滤波器状态信息只被重置到零，协方差趋于无穷，故障检测和隔离能力很差;减少了数据通讯量，计算简单。2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法优、缺点2 组合导航基本原理和方法优、缺点主滤波器分配

42、到全部信息，故障检测和隔离能力强;子滤波器状态信息只被重置到零，协方差趋于无穷，故障检测和隔离能力很差;减少了数据通讯量，计算简单。2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法优、缺点2 组合导航基本原理和方法LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合（2）第二类（ 有重置）2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法LF2LF1LFN主滤波器子2 组合导航基本原理和方法优、缺点主滤波器与子滤波器之间平均分配信息；融合后全局滤波精度高，局部因为有全局滤波反馈，精度也提高了；子滤波器故障检测与隔离性能好；主滤波器的故障检测与隔离

43、性能中等；一个子系统故障后，主滤波器受污染，隔离后必须重新初始化主滤波器。2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法优、缺点2 组合导航基本原理和方法LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合（3）第三类（ 有重置）2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法LF2LF1LFN主滤波器子2 组合导航基本原理和方法优、缺点主滤波器状态方程无信息分配，主滤波器不需要进行滤波，所以主滤波器的估计值取为全局估计，即主滤波器的故障检测与隔离能力差；子滤波器的故障检测与隔离能力与第二类结构一样。2.1 组合导航中的多传感器融合方法2 组

44、合导航基本原理和方法优、缺点2 组合导航基本原理和方法（4）第四类（ 无重置）LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法（4）第四类（ 2 组合导航基本原理和方法优、缺点各局部滤波器独立滤波，没有反馈重置带来的相互影响，提高了容错性能；由于没有全局估计，局部估计精度不高。2.1 组合导航中的多传感器融合方法2 组合导航基本原理和方法优、缺点2 组合导航基本原理和方法2.1 组合导航中的多传感器融合方法（5）第五类（ 有重置）LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量测更新2 组合导航

45、基本原理和方法（5）第五类（ 2 组合导航基本原理和方法2.1 组合导航中的多传感器融合方法优、缺点主滤波器包含所有状态信息；子滤波器的信息阵每次融合后重置为零；然后子滤波器再重新启动，即零化启动；子滤波器起到数据压缩作用，故障检测与隔离能力差；主滤波器的故障检测与隔离能力强，但是故障恢复能力差，因此要重新初始化。2 组合导航基本原理和方法优、缺点2 组合导航基本原理和方法2.1 组合导航中的多传感器融合方法LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量测更新（6）第六类（ 有重置）2 组合导航基本原理和方法LF1主滤波器子系统1子系统2 组合导航基本原理和方法2.1 组合导航中

46、的多传感器融合方法优、缺点每次信息融合后，子滤波器信息被重置到全局信息一半，协方差重置为全局估计协方差的一倍；主、子滤波器的故障检测与隔离能力中等；主滤波器的故障恢复能力差。是故障恢复能力差，因此要重新初始化。2 组合导航基本原理和方法优、缺点2 组合导航基本原理和方法2.1 组合导航中的多传感器融合方法结论：利用融合后的全局状态和协方差去反馈重置子滤波器，提高子滤波器精度，但是主滤波器也因此容易受到故障子滤波器的影响；如果不将融合后的全局状态和协方差去反馈重置子滤波器，那么就不会产生交叉污染，容错性能大大提高。2 组合导航基本原理和方法结论：2 组合导航基本原理和方法2.2 GNSS/INS

47、 组合导航2.2.1 组合导航的基本体系结构 （1）松组合法2 组合导航基本原理和方法（1）松组合法2 组合导航基本原理和方法2.2 GNSS/INS 组合导航GNSS接收机通常通过自己的Kalman滤波输出其速度和位置，这种组合导致滤波器的串联，使组合导航观测噪声时间相关（有色噪声），不满足EKF观测噪声为白噪声的基本要求，严重时可能使滤波器不稳定。几乎无冗余信息，不利于异常诊断，不利于进行随机模型改化 。 松组合的主要缺点系统结构简单，易于实现，可以大幅度提高系统的导航精度，并使INS具有动基座对准能力。 松组合的主要优点2 组合导航基本原理和方法GNSS接收机通常通过自己的2 组合导航基

48、本原理和方法2.2 GNSS/INS 组合导航（2）紧组合法2 组合导航基本原理和方法（2）紧组合法2 组合导航基本原理和方法2.2 GNSS/INS 组合导航观测量根据GNSS接收机收到的星历信息和INS输出的位置和速度信息，计算相应于INS位置的伪距和伪距率，GNSS接收机测量得到的伪距和伪距速率与INS计算值的差值。通过EKF对INS的误差和GPS接收机的误差进行最优估计，然后对INS进行输出或者反馈校正。由于不存在滤波器的级联，并可对GNSS接收机的测距误差进行建模，因此这种伪距、伪距率组合方式比位置、速度组合具有更高的组合精度。而且在可见星的个数少于4颗时也可以使用。2 组合导航基本

49、原理和方法观测量根据GNSS接收机2 组合导航基本原理和方法2.2 GNSS/INS 组合导航（3）深组合法 2 组合导航基本原理和方法（3）深组合法 2 组合导航基本原理和方法2.2 GNSS/INS 组合导航 深组合是使用惯性导航信息对GNSS接收机进行辅助导航的组合方式。主要思想：既使用滤波技术对INS的误差进行最优估计，同时使用校正后的INS速度信息对接收机的载波环、码环进行辅助跟踪，从而减小环路的等效带宽，增加GPS接收机在高动态或强干扰环境下的跟踪能力。嵌入式组合将INS和GNSS进行一体化设计，通过共用电源、时钟等进一步减小体积、降低成本和减小非同步误差的影响。2 组合导航基本原理和方法 深组合是使用惯性导航信息对2 组合导航基本原理和方法2.3 组合导航方法举例 2 组合导航基本原理和方法 3 导航技术发展趋势 制导弹药等常规武器制导化领域：20世纪90年代