卡尔曼滤波与组合导航 第四讲课件

卡尔曼滤波与组合导航

TheoryofKalmanfilterandIntegratedNavigation卡尔曼滤波与组合导航TheoryofKalmanfi1第四章最优滤波在惯性/天文/卫星组合导航系统中的应用

4.1概述4.2基于集中滤波的SINS/CNS/GPS组合导航方法4.3基于联邦滤波的SINS/CNS/GPS组合导航方法4.4SINS/CNS/GPS半物理仿真系统第四章最优滤波在惯性/天文/卫星组合导航系统中的应用4.24.1概述我国对新一代战略导弹的命中精度要求进入100米级

导航制导系统（其误差占命中误差的70%）我国现役战略武器命中精度比要求值低5~10倍惯性导航与组合导航核心技术陆、海、空、天武器系统

广泛应用于

精确打击

新军事变革

迫切需要精确打击——赢得现代战争胜利的重要保障!瓶颈导致弹道导弹4.1概述我国对新一代战略导弹的命中精度要求进入1034.1概述精确打击——赢得现代战争胜利的重要保障!弹道导弹巡航导弹地空导弹远程空空导弹惯性导航与组合导航核心技术陆、海、空、天武器系统

广泛应用于

精确打击

新军事变革

迫切需要制导炸弹1991年：精确制导弹药比例仅为20%；16～18架次/目标2004年：上升到60%～70%；1个架次/目标提高导航系统性能、同时降低成本关键组合导航4.1概述精确打击——赢得现代战争胜利的重要保障!44.1概述优点：★完全自主、运动参数完备★短时精度高★广泛应用于武器装备缺点：误差积累、成本较高优点：★全天候、高精度★误差不积累缺点：缺姿态信息、易被干扰优点：★自主、高精度★误差不积累缺点：无位置速度、气候受限位置速度姿态位置速度姿态提供卫星导航系统提供天文定姿系统特点特点惯性导航系统提供特点几种常用的导航系统的优缺点4.1概述优点：优点：★全天候、高精度优点：★自54.1概述卫星导航系统惯性导航系统优点：★综合惯性和卫星的优点★位置、速度不随时间发散部分姿态角和惯性器件误差

可观测度低

长时间工作姿态精度下降信息融合特点SINS/GPS组合导航的优缺点4.1概述卫星惯性优点：64.1概述天文定姿系统惯性导航系统优点：★完全自主、实时★运动参数完备★姿态误差不随时间发散缺点：位置、速度误差随时间积累信息融合特点SINS/CNS组合导航的优缺点4.1概述天文惯性优点：7优势互补！惯性/卫星/天文组合导航系统4.1概述取长补短！最理想的组合导航系统卫星导航系统天文定姿系统惯性导航系统是SINS/CNS/GPS组合导航的优缺点优势互补！惯性/卫星/天文4.1概述取长补短！最理想84.1概述卫星导航系统天文定姿系统SINS/CNS/GPS组合导航基本原理最优滤波惯性导航系统位置、速度和姿态各子系统误差位置速度观测量姿态观测量4.1概述卫星天文SI94.2基于集中滤波的SINS/CNS/GPS组合导航利用卡尔曼滤波技术对组合导航系统进行最优估计组合有两种途径：集中式滤波分散化滤波

利用一个卡尔曼滤波器来集中地处理所有导航子系统的信息4.2.1集中滤波概述4.2基于集中滤波的SINS/CNS/GPS组合导航10(1)集中式滤波器结构滤波器子系统1参考系统时间更新最优融合子系统2子系统24.2.1集中滤波概述(1)集中式滤波器结构滤波器子系统1参考系统时间更新最优融11(2)集中式滤波器优点(1)可以给出最优的误差估计；(2)只有一个滤波器，结构简单，工程中容易实现。(3)目前国内采用的仍然主要是集中式滤波。4.2.1集中滤波概述(2)集中式滤波器优点(1)可以给出最优的误差估计；(12(3)集中式滤波器缺点集中式滤波理论上可以给出误差估计的最优估计，但存在着如下缺点：(1)状态维数高，计算负担重，不利于滤波实时运行，状态维数高会带来“维数灾难”；(2)容错性能差，不利于故障诊断，信息污染问题。4.2.1集中滤波概述(3)集中式滤波器缺点集中式滤波理论上可以13(1)基于集中滤波器的SINS/CNS/GPS组合导航系统结构4.2.2基于集中滤波的SINS/CNS/GPS组合导航卫星导航系统天文定姿系统最优滤波惯性导航系统位置、速度和姿态各子系统误差-位置速度+-姿态观测量+(1)基于集中滤波器的SINS/CNS/GPS组合导航系统结14(3)系统误差模型4.2.2基于集中滤波的SINS/CNS/GPS组合导航加速度误差方程(3)系统误差模型4.2.2基于集中滤波的SINS/15(3)系统误差模型4.2.2基于集中滤波的SINS/CNS/GPS组合导航速度误差方程(3)系统误差模型4.2.2基于集中滤波的SINS/16(3)系统误差模型4.2.2基于集中滤波的SINS/CNS/GPS组合导航平台失准角误差方程(3)系统误差模型4.2.2基于集中滤波的SINS/17(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航X系统状态向量(4)SINS/CNS/GPS集中滤波器设计系统状态方程418(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航平台误差角速度误差位置误差陀螺随机常值漂移加计随机常值偏置不是姿态误差角为了简化此处未将CNS和GPS的误差建为系统状态

(4)SINS/CNS/GPS集中滤波器设计系统状态方程419(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航W系统噪声向量陀螺随机漂移加计随机误差(4)SINS/CNS/GPS集中滤波器设计系统状态方程420(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航状态转移矩阵(4)SINS/CNS/GPS集中滤波器设计系统状态方程421(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航(4)SINS/CNS/GPS集中滤波器设计系统状态方程422(4)SINS/CNS/GPS集中滤波器设计量测方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航观测量由星敏感器获得平台误差角由GPS获得位置速度误差(4)SINS/CNS/GPS集中滤波器设计量测方程4.223(4)SINS/CNS/GPS集中滤波器设计量测方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航观测量量测矩阵(4)SINS/CNS/GPS集中滤波器设计量测方程4.224(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集中滤波的SINS/CNS/GPS组合导航导弹垂直发射，发射方位角为90°初始纬度为39.984°，经度为116.344°初始航向角为0°，俯仰角为90°，横滚角为0°导弹导航流程：导弹轨迹分为主动段和被动段。40秒飞出大气层后天文开始工作，在158秒主动段关机，进入末修段，180秒后导弹自由飞行。

(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集25(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集中滤波的SINS/CNS/GPS组合导航导弹射程：4373.446km关机点速度：Vx=4118.629m/s；Vy=3266.905m/s；Vz=-141.3815m/s红色为主动段(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集26仿真条件1）陀螺漂移取0.1度/小时，加计偏置取10µg2）选择不同的方位失准角（30角分、6角分）3）水平失调角2角秒5）星敏感器精度：分别取3“、6“和10“°6）导弹射程4373.446km北航研制惯性/星光/卫星半物理仿真系统仿真条件北航研制惯性/星光/卫星半物理仿真系统27仿真结果仿真结果28结论优点：惯性/天文/卫星组合导航有很高的定位精度可大大降低导弹对惯导系统要求，降低了成本集中式缺点：状态维数高，计算负担重，不利于滤波实时运行，如建立更高阶误差模型，将会带来“维数灾难”；容错性能差，不利于故障诊断，信息污染问题。结论优点：294.3基于联邦滤波的SINS/CNS/GPS组合导航利用卡尔曼滤波技术对组合导航系统进行最优估计组合的两种途径：分散化滤波

1971年Pearson提出分散滤波的概念利用子滤波器处理子系统的信息通过全局滤波器实现所有子系统的信息融合集中式滤波4.3.1联邦滤波概述4.3基于联邦滤波的SINS/CNS/GPS组合导航30分散滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合4.3.1联邦滤波概述分散滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系31分散滤波器优、缺点优点

(1)全局滤波仍然较复杂；(2)算法基于各测量值是不相关的假设。缺点计算量小，容错性好。4.3.1联邦滤波概述分散滤波器优、缺点优点(1)全局滤波仍然较32为更清楚地理解联邦滤波的特点和要解决的问题，首先介绍一种简单的分散滤波，它的局部滤波和全局滤波都是最优的。假定系统的状态方程和量测方程为其中的协方差阵为,的协方差阵为

一种简单的分散滤波4.3.1联邦滤波概述为更清楚地理解联邦滤波的特点和要解决的问题，33子系统的状态方程和量测方程为其中的协方差阵为,的协方差阵为

上式表示总系统利用了所有子系统的量测信息。

一种简单的分散滤波4.3.1联邦滤波概述子系统的状态方程和量测方程为其中的协方差阵为34假设各子系统的量测相互独立，且是的一部分

一种简单的分散滤波4.3.1联邦滤波概述假设各子系统的量测相互独立，且是的35一种简单的分散滤波4.3.1联邦滤波概述一种简单的分散滤波4.3.1联邦滤波概述36全局滤波的量测更新可用局部滤波表示；时间更新仍需用全局滤波方程；子滤波和全局滤波都是最优的。可见：一种简单的分散滤波上述分散滤波方法存在以下缺点：全局滤波的合成算法比较复杂，不仅用到了子滤波器的滤波值和协方差，还用到了它们的预报值；算法基于各量测值是不相关的。4.3.1联邦滤波概述全局滤波的量测更新可用局部滤波表示；可见：一种简单的分散371988年Carlson提出了联邦滤波器(FederatedFilter)属于一种分散滤波方法设计灵活

计算量小

容错性能好

特点4.3.1联邦滤波概述1988年Carlson提出了联邦滤波器(Federated38联邦滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合4.3.1联邦滤波概述联邦滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系39联邦滤波要解决的问题滤波器的容错性能要好，检测并隔离故障子系统，并进行系统重构；滤波的精度要高；由局部滤波到全局滤波的融合算法要简单，计算量小。

上述几个性能要求是矛盾的。要容错性能好，有时就要牺牲一些精度。为了解决这几个性能要求，联邦滤波中用了“信息分配”原则，获得最佳折中的性能。4.3.1联邦滤波概述联邦滤波要解决的问题滤波器的容错性能要好，检测并隔离故障子系40信息分配原则首先说明什么是信息，系统中有两类信息：（1）状态运动方程的信息状态方程的信息量是与系统噪声的方差成反比的，因此可用Q-1表示状态方程信息量；状态初值的信息也是状态方程的信息，可用初值估计的协方差阵的逆P-1(0)表示。（2）量测方程的信息量测方程的信息可用量测噪声协方差的逆R-1表示。信息分配原则首先说明什么是信息，系统中有两类信息：（1）状态41信息分配原则（续）假设将系统噪声总的信息量Q-1分配到各局部滤波器和主滤波器中去,即故:根据信息守恒状态估计初值P-1(0)也可按上述方法分配,可得信息分配原则（续）假设将系统噪声总的信息量Q42联邦滤波器设计步骤（1）将子滤波器和主滤波器的初始估计协方差设置为组合系统初始值的

倍,满足信息守恒原则；（2）将子滤波器和主滤波器的系统噪声协方差设置为组合系统系统噪声协方差的

倍,满足信息守恒原则；（3）各子滤波器处理自己的量测信息,获得局部最优估计；（4）得到局部估计和主滤波器的估计后按下式最优合成：（5）用全局滤波解来重置各子滤波器和主滤波器的滤波值和协方差阵。联邦滤波器设计步骤（1）将子滤波器和主滤波器的初始估计协方差43联邦滤波器的结构与性能分析设计联邦滤波器，不同信息分配系数会有不同的结构和特性(容错性、最优性、计算量)。通常有6种不同的结构。（1）第一类（零化式重置）

LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合联邦滤波器的结构与性能分析设计联邦滤44（1）第一类（零化式重置）优、缺点主滤波器分配到全部信息，故障检测和隔离能力强;子滤波器状态信息只被重置到零，协方差趋于无穷，故障检测和隔离能力很差;减少了数据通讯量，计算简单。联邦滤波器的结构与性能分析（续）（1）第一类（45LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合（2）第二类（有重置）联邦滤波器的结构与性能分析（续）LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时46优、缺点主滤波器与子滤波器之间平均分配信息；融合后全局滤波精度高，局部因为有全局滤波反馈，精度也提高了；子滤波器故障检测与隔离性能好；主滤波器的故障检测与隔离性能中等；一个子系统故障后，主滤波器受污染，隔离后必须重新初始化主滤波器。（2）第二类（有重置）联邦滤波器的结构与性能分析（续）优、缺点（2）第二类（47LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合（3）第三类（有重置）联邦滤波器的结构与性能分析（续）LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时48优、缺点主滤波器状态方程无信息分配，主滤波器不需要进行滤波，所以主滤波器的估计值取为全局估计，即主滤波器的故障检测与隔离能力差；子滤波器的故障检测与隔离能力与第二类结构一样。（3）第三类（有重置）联邦滤波器的结构与性能分析（续）优、缺点（3）第三类（49（4）第四类（无重置）LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合联邦滤波器的结构与性能分析（续）（4）第四类（50（4）第四类（无重置）优、缺点各局部滤波器独立滤波，没有反馈重置带来的相互影响，提高了容错性能；由于没有全局估计，局部估计精度不高。联邦滤波器的结构与性能分析（续）（4）第四类（51（5）第五类（有重置）联邦滤波器的结构与性能分析（续）LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量测更新（5）第五类（52（5）第五类（有重置）优、缺点主滤波器包含所有状态信息；子滤波器的信息阵每次融合后重置为零；然后子滤波器再重新启动，即零化启动；子滤波器起到数据压缩作用，故障检测与隔离能力差；主滤波器的故障检测与隔离能力强，但是故障恢复能力差，因此要重新初始化。联邦滤波器的结构与性能分析（续）（5）第五类（53LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量测更新（6）第六类（有重置）联邦滤波器的结构与性能分析（续）LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量54（6）第六类（有重置）优、缺点每次信息融合后，子滤波器信息被重置到全局信息一半，协方差重置为全局估计协方差的一倍；主、子滤波器的故障检测与隔离能力中等；主滤波器的故障恢复能力差。是故障恢复能力差，因此要重新初始化。联邦滤波器的结构与性能分析（续）（6）第六类（55结论：利用融合后的全局状态和协方差去反馈重置子滤波器，提高子滤波器精度，但是主滤波器也因此容易受到故障子滤波器的影响；如果不将融合后的全局状态和协方差去反馈重置子滤波器，那么就不会产生交叉污染，容错性能大大提高，为全局估计协方差的一倍。联邦滤波器的结构与性能分析（续）结论：联邦滤波器的结构与性能分析（续）564.3.2基于联邦滤波的SINS/CNS/GPS组合导航本实验室SINS/GPS/CNS组合导航系统实物图：SINSCNS半物理仿真系统GPS4.3.2基于联邦滤波的SINS/CNS/GPS组合导航574.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法基于联邦滤波的SINS/CNS/GPS系统结构主滤波器CNSGPSSINS时间更新最优融合优化信息分配优化信息分配子滤波器1子滤波器2参考系统子系统1子系统24.3.2基于联邦滤波的SINS/CNS/GPS组合导航58状态方程由SINS的误差方程和惯性器件的误差方程组成，GPS仅提供部分观测量，不提供状态变量。姿态误差方程为

4.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法滤波器设计——子滤波器1SINS/GPS

状态方程由SINS的误差方程和惯性器件的误差方程组成，GPS594.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法状态方程速度误差方程为

滤波器设计——子滤波器1SINS/GPS

4.3.2基于联邦滤波的SINS/CNS/GPS组合导航604.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法

位置误差方程为惯性器件的误差方程：滤波器设计——子滤波器1SINS/GPS

状态方程4.3.2基于联邦滤波的SINS/CNS/GPS组合导航614.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法滤波器设计——子滤波器1SINS/GPS

量测方程

SINS/GPS子系统采用浅组合的方式，GPS提供位置和速度信息，采用SINS与GPS的位置和速度之差作为卡尔曼滤波的量测信息。量测方程：式中：4.3.2基于联邦滤波的SINS/CNS/GPS组合导航624.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法状态方程：同子滤波器1采用经过换算得到的数学平台误差角作为卡尔曼滤波器的观测量，量测方程：量测方程：式中：——观测量——量测矩阵滤波器设计——子滤波器2SINS/CNS

4.3.2基于联邦滤波的SINS/CNS/GPS组合导航63提出一种基于优化信息分配因子的联邦滤波方法状态估计误差协方差阵如果测量系统是可观测的，那么协方差阵所有元素都是限定的。状态估计误差可以直接由协方差阵P如下计算：状态估计器的性能由这个状态估计误差来评价。4.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法提出一种基于优化信息分配因子的联邦滤波方法状态估计误差64相当于把估计误差向量长度最小化并且希望是P的迹最小，因为它是在估计所有状态向量元素的均方误差之和，如果这里没有不确定过程和量测噪声协方差，使用标准卡尔曼滤波性能指标C获得全局最小值。但是如果Q、R不确定，C不能获得最小值，这里假设没有不确定的Q、R，成本最小化函数相当于的极大值。信息融合分配因子为提出一种基于优化信息分配因子的联邦滤波方法（续）4.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法相当于把估计误差向量长度最小化并且希望是P的65仿真轨迹：

基于弹道导弹飞行，前40秒SINS/GPS组合导航，

40秒以后SINS/GPS/CNS组合导航提出一种基于优化信息分配因子的联邦滤波方法（续）4.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法仿真轨迹：

基于弹道导弹飞行，前40秒SINS/GPS组合导66仿真结果图：AttitudeerrorsGyrosdriftsAccelerometersbiases仿真结果图：AttitudeerrorsGyros67仿真结果图：VelocityerrorcompensationPositionerrorcompensation仿真结果图：Velocityerror68基于优化信息分配因子的联邦滤波方法仿真结论4.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法能够达到滤波的全局最优；可以估算并补偿每个轴的误差角、陀螺漂移以及加速度计偏置；CNS可以很好地估计并补偿由陀螺漂移产生的失准角误差，而且能抑制由失准角产生的速度和位置误差；GPS可以修正速度位置误差，估计和补偿加速度计的偏置。基于优化信息分配因子的联邦滤波方法仿真结论4.3.2694.4SINS/CNS/GPS半物理仿真系统为什么要构建半物理仿真系统半物理仿真系统验证算法测试系统性能的最为有效的平台优点：（1）减少飞行试验次数（2）降低试验成本（3）缩短系统研制周期（4）可对算法和系统性能进行快速有效地验证和测试4.4SINS/CNS/GPS半物理仿真系统为什么704.4SINS/CNS/GPS半物理仿真系统SINS/CNS/GPS半物理仿真系统的基本组成轨迹发生器天文子系统

生成载体轨道数据，模拟载体运行轨迹惯性子系统

卫星子系统

组合子系统显示子系统采用真实惯导系统，提供真实误差数据采用星模拟器和星敏感器模拟器，提供天文姿态数据采用真实卫星导航系统，提供真实数据接收惯性、天文、卫星子系统的数据，实现高精度组合导航对惯性、天文、卫星子系统信息和组合导航信息进行直观显示4.4SINS/CNS/GPS半物理仿真系统SINS71高精度位置、

速度和姿态4.4SINS/CNS/GPS半物理仿真系统SINS/CNS/GPS半物理仿真系统的基本流程轨道发生器组合子系统星光模拟器星图模拟终端星图识别终端星光敏感器模拟的

平行星光生成的轨道数据天文子系统惯性子系统卫星子系统显示子系统高精度位置、

速度和姿态4.4SINS/CNS/GP724.4SINS/CNS/GPS半物理仿真系统SINS/CNS/GPS半物理仿真系统关键技术星图模拟技术系统实现技术生成载体轨道数据，模拟载体运行轨迹星图识别技术

采用真实惯导系统，提供真实误差数据采用星模拟器和星敏感器模拟器，提供天文姿态数据4.4SINS/CNS/GPS半物理仿真系统SINS734.4SINS/CNS/GPS半物理仿真系统星图模拟技术专用面光源液晶光阀平行光管星敏感器液晶光阀平行光管液晶光阀基本星表星图模拟系统简图4.4SINS/CNS/GPS半物理仿真系统星图模拟74全天球基本星表从基本星表分区中选取的恒星点集分别对这两个点集提取特征信息，通过先进的匹配识别算法，完成对星图的快速识别恒星敏感器4.4SINS/CNS/GPS半物理仿真系统星图识别技术全天球基本星表从基本星表分区中选取的恒星点集分别对这两个点集754.4SINS/CNS/GPS半物理仿真系统系统实现北航研制惯性/星光/卫星半物理仿真系统4.4SINS/CNS/GPS半物理仿真系统系统实现76谢谢谢谢77卡尔曼滤波与组合导航

TheoryofKalmanfilterandIntegratedNavigation卡尔曼滤波与组合导航TheoryofKalmanfi78第四章最优滤波在惯性/天文/卫星组合导航系统中的应用

4.1概述4.2基于集中滤波的SINS/CNS/GPS组合导航方法4.3基于联邦滤波的SINS/CNS/GPS组合导航方法4.4SINS/CNS/GPS半物理仿真系统第四章最优滤波在惯性/天文/卫星组合导航系统中的应用4.794.1概述我国对新一代战略导弹的命中精度要求进入100米级

导航制导系统（其误差占命中误差的70%）我国现役战略武器命中精度比要求值低5~10倍惯性导航与组合导航核心技术陆、海、空、天武器系统

广泛应用于

精确打击

新军事变革

迫切需要精确打击——赢得现代战争胜利的重要保障!瓶颈导致弹道导弹4.1概述我国对新一代战略导弹的命中精度要求进入10804.1概述精确打击——赢得现代战争胜利的重要保障!弹道导弹巡航导弹地空导弹远程空空导弹惯性导航与组合导航核心技术陆、海、空、天武器系统

广泛应用于

精确打击

新军事变革

迫切需要制导炸弹1991年：精确制导弹药比例仅为20%；16～18架次/目标2004年：上升到60%～70%；1个架次/目标提高导航系统性能、同时降低成本关键组合导航4.1概述精确打击——赢得现代战争胜利的重要保障!814.1概述优点：★完全自主、运动参数完备★短时精度高★广泛应用于武器装备缺点：误差积累、成本较高优点：★全天候、高精度★误差不积累缺点：缺姿态信息、易被干扰优点：★自主、高精度★误差不积累缺点：无位置速度、气候受限位置速度姿态位置速度姿态提供卫星导航系统提供天文定姿系统特点特点惯性导航系统提供特点几种常用的导航系统的优缺点4.1概述优点：优点：★全天候、高精度优点：★自824.1概述卫星导航系统惯性导航系统优点：★综合惯性和卫星的优点★位置、速度不随时间发散部分姿态角和惯性器件误差

可观测度低

长时间工作姿态精度下降信息融合特点SINS/GPS组合导航的优缺点4.1概述卫星惯性优点：834.1概述天文定姿系统惯性导航系统优点：★完全自主、实时★运动参数完备★姿态误差不随时间发散缺点：位置、速度误差随时间积累信息融合特点SINS/CNS组合导航的优缺点4.1概述天文惯性优点：84优势互补！惯性/卫星/天文组合导航系统4.1概述取长补短！最理想的组合导航系统卫星导航系统天文定姿系统惯性导航系统是SINS/CNS/GPS组合导航的优缺点优势互补！惯性/卫星/天文4.1概述取长补短！最理想854.1概述卫星导航系统天文定姿系统SINS/CNS/GPS组合导航基本原理最优滤波惯性导航系统位置、速度和姿态各子系统误差位置速度观测量姿态观测量4.1概述卫星天文SI864.2基于集中滤波的SINS/CNS/GPS组合导航利用卡尔曼滤波技术对组合导航系统进行最优估计组合有两种途径：集中式滤波分散化滤波

利用一个卡尔曼滤波器来集中地处理所有导航子系统的信息4.2.1集中滤波概述4.2基于集中滤波的SINS/CNS/GPS组合导航87(1)集中式滤波器结构滤波器子系统1参考系统时间更新最优融合子系统2子系统24.2.1集中滤波概述(1)集中式滤波器结构滤波器子系统1参考系统时间更新最优融88(2)集中式滤波器优点(1)可以给出最优的误差估计；(2)只有一个滤波器，结构简单，工程中容易实现。(3)目前国内采用的仍然主要是集中式滤波。4.2.1集中滤波概述(2)集中式滤波器优点(1)可以给出最优的误差估计；(89(3)集中式滤波器缺点集中式滤波理论上可以给出误差估计的最优估计，但存在着如下缺点：(1)状态维数高，计算负担重，不利于滤波实时运行，状态维数高会带来“维数灾难”；(2)容错性能差，不利于故障诊断，信息污染问题。4.2.1集中滤波概述(3)集中式滤波器缺点集中式滤波理论上可以90(1)基于集中滤波器的SINS/CNS/GPS组合导航系统结构4.2.2基于集中滤波的SINS/CNS/GPS组合导航卫星导航系统天文定姿系统最优滤波惯性导航系统位置、速度和姿态各子系统误差-位置速度+-姿态观测量+(1)基于集中滤波器的SINS/CNS/GPS组合导航系统结91(3)系统误差模型4.2.2基于集中滤波的SINS/CNS/GPS组合导航加速度误差方程(3)系统误差模型4.2.2基于集中滤波的SINS/92(3)系统误差模型4.2.2基于集中滤波的SINS/CNS/GPS组合导航速度误差方程(3)系统误差模型4.2.2基于集中滤波的SINS/93(3)系统误差模型4.2.2基于集中滤波的SINS/CNS/GPS组合导航平台失准角误差方程(3)系统误差模型4.2.2基于集中滤波的SINS/94(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航X系统状态向量(4)SINS/CNS/GPS集中滤波器设计系统状态方程495(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航平台误差角速度误差位置误差陀螺随机常值漂移加计随机常值偏置不是姿态误差角为了简化此处未将CNS和GPS的误差建为系统状态

(4)SINS/CNS/GPS集中滤波器设计系统状态方程496(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航W系统噪声向量陀螺随机漂移加计随机误差(4)SINS/CNS/GPS集中滤波器设计系统状态方程497(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航状态转移矩阵(4)SINS/CNS/GPS集中滤波器设计系统状态方程498(4)SINS/CNS/GPS集中滤波器设计系统状态方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航(4)SINS/CNS/GPS集中滤波器设计系统状态方程499(4)SINS/CNS/GPS集中滤波器设计量测方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航观测量由星敏感器获得平台误差角由GPS获得位置速度误差(4)SINS/CNS/GPS集中滤波器设计量测方程4.2100(4)SINS/CNS/GPS集中滤波器设计量测方程4.2.2基于集中滤波的SINS/CNS/GPS组合导航观测量量测矩阵(4)SINS/CNS/GPS集中滤波器设计量测方程4.2101(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集中滤波的SINS/CNS/GPS组合导航导弹垂直发射，发射方位角为90°初始纬度为39.984°，经度为116.344°初始航向角为0°，俯仰角为90°，横滚角为0°导弹导航流程：导弹轨迹分为主动段和被动段。40秒飞出大气层后天文开始工作，在158秒主动段关机，进入末修段，180秒后导弹自由飞行。

(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集102(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集中滤波的SINS/CNS/GPS组合导航导弹射程：4373.446km关机点速度：Vx=4118.629m/s；Vy=3266.905m/s；Vz=-141.3815m/s红色为主动段(5)计算机仿真（弹道导弹）仿真条件4.2.2基于集103仿真条件1）陀螺漂移取0.1度/小时，加计偏置取10µg2）选择不同的方位失准角（30角分、6角分）3）水平失调角2角秒5）星敏感器精度：分别取3“、6“和10“°6）导弹射程4373.446km北航研制惯性/星光/卫星半物理仿真系统仿真条件北航研制惯性/星光/卫星半物理仿真系统104仿真结果仿真结果105结论优点：惯性/天文/卫星组合导航有很高的定位精度可大大降低导弹对惯导系统要求，降低了成本集中式缺点：状态维数高，计算负担重，不利于滤波实时运行，如建立更高阶误差模型，将会带来“维数灾难”；容错性能差，不利于故障诊断，信息污染问题。结论优点：1064.3基于联邦滤波的SINS/CNS/GPS组合导航利用卡尔曼滤波技术对组合导航系统进行最优估计组合的两种途径：分散化滤波

1971年Pearson提出分散滤波的概念利用子滤波器处理子系统的信息通过全局滤波器实现所有子系统的信息融合集中式滤波4.3.1联邦滤波概述4.3基于联邦滤波的SINS/CNS/GPS组合导航107分散滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合4.3.1联邦滤波概述分散滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系108分散滤波器优、缺点优点

(1)全局滤波仍然较复杂；(2)算法基于各测量值是不相关的假设。缺点计算量小，容错性好。4.3.1联邦滤波概述分散滤波器优、缺点优点(1)全局滤波仍然较109为更清楚地理解联邦滤波的特点和要解决的问题，首先介绍一种简单的分散滤波，它的局部滤波和全局滤波都是最优的。假定系统的状态方程和量测方程为其中的协方差阵为,的协方差阵为

一种简单的分散滤波4.3.1联邦滤波概述为更清楚地理解联邦滤波的特点和要解决的问题，110子系统的状态方程和量测方程为其中的协方差阵为,的协方差阵为

上式表示总系统利用了所有子系统的量测信息。

一种简单的分散滤波4.3.1联邦滤波概述子系统的状态方程和量测方程为其中的协方差阵为111假设各子系统的量测相互独立，且是的一部分

一种简单的分散滤波4.3.1联邦滤波概述假设各子系统的量测相互独立，且是的112一种简单的分散滤波4.3.1联邦滤波概述一种简单的分散滤波4.3.1联邦滤波概述113全局滤波的量测更新可用局部滤波表示；时间更新仍需用全局滤波方程；子滤波和全局滤波都是最优的。可见：一种简单的分散滤波上述分散滤波方法存在以下缺点：全局滤波的合成算法比较复杂，不仅用到了子滤波器的滤波值和协方差，还用到了它们的预报值；算法基于各量测值是不相关的。4.3.1联邦滤波概述全局滤波的量测更新可用局部滤波表示；可见：一种简单的分散1141988年Carlson提出了联邦滤波器(FederatedFilter)属于一种分散滤波方法设计灵活

计算量小

容错性能好

特点4.3.1联邦滤波概述1988年Carlson提出了联邦滤波器(Federated115联邦滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合4.3.1联邦滤波概述联邦滤波器结构LF2LF1LFN主滤波器子系统2子系统1子系116联邦滤波要解决的问题滤波器的容错性能要好，检测并隔离故障子系统，并进行系统重构；滤波的精度要高；由局部滤波到全局滤波的融合算法要简单，计算量小。

上述几个性能要求是矛盾的。要容错性能好，有时就要牺牲一些精度。为了解决这几个性能要求，联邦滤波中用了“信息分配”原则，获得最佳折中的性能。4.3.1联邦滤波概述联邦滤波要解决的问题滤波器的容错性能要好，检测并隔离故障子系117信息分配原则首先说明什么是信息，系统中有两类信息：（1）状态运动方程的信息状态方程的信息量是与系统噪声的方差成反比的，因此可用Q-1表示状态方程信息量；状态初值的信息也是状态方程的信息，可用初值估计的协方差阵的逆P-1(0)表示。（2）量测方程的信息量测方程的信息可用量测噪声协方差的逆R-1表示。信息分配原则首先说明什么是信息，系统中有两类信息：（1）状态118信息分配原则（续）假设将系统噪声总的信息量Q-1分配到各局部滤波器和主滤波器中去,即故:根据信息守恒状态估计初值P-1(0)也可按上述方法分配,可得信息分配原则（续）假设将系统噪声总的信息量Q119联邦滤波器设计步骤（1）将子滤波器和主滤波器的初始估计协方差设置为组合系统初始值的

倍,满足信息守恒原则；（2）将子滤波器和主滤波器的系统噪声协方差设置为组合系统系统噪声协方差的

倍,满足信息守恒原则；（3）各子滤波器处理自己的量测信息,获得局部最优估计；（4）得到局部估计和主滤波器的估计后按下式最优合成：（5）用全局滤波解来重置各子滤波器和主滤波器的滤波值和协方差阵。联邦滤波器设计步骤（1）将子滤波器和主滤波器的初始估计协方差120联邦滤波器的结构与性能分析设计联邦滤波器，不同信息分配系数会有不同的结构和特性(容错性、最优性、计算量)。通常有6种不同的结构。（1）第一类（零化式重置）

LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合联邦滤波器的结构与性能分析设计联邦滤121（1）第一类（零化式重置）优、缺点主滤波器分配到全部信息，故障检测和隔离能力强;子滤波器状态信息只被重置到零，协方差趋于无穷，故障检测和隔离能力很差;减少了数据通讯量，计算简单。联邦滤波器的结构与性能分析（续）（1）第一类（122LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合（2）第二类（有重置）联邦滤波器的结构与性能分析（续）LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时123优、缺点主滤波器与子滤波器之间平均分配信息；融合后全局滤波精度高，局部因为有全局滤波反馈，精度也提高了；子滤波器故障检测与隔离性能好；主滤波器的故障检测与隔离性能中等；一个子系统故障后，主滤波器受污染，隔离后必须重新初始化主滤波器。（2）第二类（有重置）联邦滤波器的结构与性能分析（续）优、缺点（2）第二类（124LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合（3）第三类（有重置）联邦滤波器的结构与性能分析（续）LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时125优、缺点主滤波器状态方程无信息分配，主滤波器不需要进行滤波，所以主滤波器的估计值取为全局估计，即主滤波器的故障检测与隔离能力差；子滤波器的故障检测与隔离能力与第二类结构一样。（3）第三类（有重置）联邦滤波器的结构与性能分析（续）优、缺点（3）第三类（126（4）第四类（无重置）LF2LF1LFN主滤波器子系统2子系统1子系统N参考系统时间更新最优融合联邦滤波器的结构与性能分析（续）（4）第四类（127（4）第四类（无重置）优、缺点各局部滤波器独立滤波，没有反馈重置带来的相互影响，提高了容错性能；由于没有全局估计，局部估计精度不高。联邦滤波器的结构与性能分析（续）（4）第四类（128（5）第五类（有重置）联邦滤波器的结构与性能分析（续）LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量测更新（5）第五类（129（5）第五类（有重置）优、缺点主滤波器包含所有状态信息；子滤波器的信息阵每次融合后重置为零；然后子滤波器再重新启动，即零化启动；子滤波器起到数据压缩作用，故障检测与隔离能力差；主滤波器的故障检测与隔离能力强，但是故障恢复能力差，因此要重新初始化。联邦滤波器的结构与性能分析（续）（5）第五类（130LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量测更新（6）第六类（有重置）联邦滤波器的结构与性能分析（续）LF1主滤波器子系统1子系统2参考系统时间更新最优融合最优量131（6）第六类（有重置）优、缺点每次信息融合后，子滤波器信息被重置到全局信息一半，协方差重置为全局估计协方差的一倍；主、子滤波器的故障检测与隔离能力中等；主滤波器的故障恢复能力差。是故障恢复能力差，因此要重新初始化。联邦滤波器的结构与性能分析（续）（6）第六类（132结论：利用融合后的全局状态和协方差去反馈重置子滤波器，提高子滤波器精度，但是主滤波器也因此容易受到故障子滤波器的影响；如果不将融合后的全局状态和协方差去反馈重置子滤波器，那么就不会产生交叉污染，容错性能大大提高，为全局估计协方差的一倍。联邦滤波器的结构与性能分析（续）结论：联邦滤波器的结构与性能分析（续）1334.3.2基于联邦滤波的SINS/CNS/GPS组合导航本实验室SINS/GPS/CNS组合导航系统实物图：SINSCNS半物理仿真系统GPS4.3.2基于联邦滤波的SINS/CNS/GPS组合导航1344.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法基于联邦滤波的SINS/CNS/GPS系统结构主滤波器CNSGPSSINS时间更新最优融合优化信息分配优化信息分配子滤波器1子滤波器2参考系统子系统1子系统24.3.2基于联邦滤波的SINS/CNS/GPS组合导航135状态方程由SINS的误差方程和惯性器件的误差方程组成，GPS仅提供部分观测量，不提供状态变量。姿态误差方程为

4.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法滤波器设计——子滤波器1SINS/GPS

状态方程由SINS的误差方程和惯性器件的误差方程组成，GPS1364.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法状态方程速度误差方程为

滤波器设计——子滤波器1SINS/GPS

4.3.2基于联邦滤波的SINS/CNS/GPS组合导航1374.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法

位置误差方程为惯性器件的误差方程：滤波器设计——子滤波器1SINS/GPS

状态方程4.3.2基于联邦滤波的SINS/CNS/GPS组合导航1384.3.2基于联邦滤波的SINS/CNS/GPS组合导航方法滤波器设计——子滤波器1SINS/GPS

量测方程

SINS/GPS子系统采用浅组合的方式，GPS提供位置和速度信息，采用SINS与GPS的位置和速度之差作为卡尔曼滤波的量测信息。量测方程：式中：4.3.2基于联邦滤波的SINS/CNS/GPS组合导航1