1、卡尔曼滤波与组合导航,房建成 教授,主 讲：,Theory of Kalman filter and Integrated Navigation,,EMAIL：,第二章 SINS/GPS组合导航系统技术,2.1 捷联惯性导航系统（SINS）,2.2 GPS动态滤波,2.3 几种最优估计方法,第二章 SINS/GPS组合导航系统技术,2.1 捷联惯性导航系统（SINS）,省去了复杂的稳定平台，结构简单 体积小 重量轻 成本低 新型陀螺的出现和计算机技术的发展推动了SINS技术,2.1.1 SINS的优点?,捷联惯导是惯性技术的主要发展方向！,2.1 捷联惯性导航系统,2.1.2 捷联惯导主要组成
2、部分及功能,敏感角速度和 加速度,信号采集 误差补偿 导航解算,2.1.3 捷联惯导系统的误差,确定性误差,随机误差,1、IMU误差标定（实验室）,2、地面静基座快速精确初始对准,3、SINS/GPS组合滤波,4、空中机动对准,最优滤波,提高精度减小或抑制误差,加速度通道偏置误差刻度因数 安装误差,角速度通道常值误差标度因数与g有关项安装误差,惯性测量单元,误差,占SINS误差 的90 必须标定补偿,2.1.4 IMU的误差标定与补偿,IMU标定的基本原理,建立IMU综合误差方程,转台试验，获取IMU输入输出数据对,代入IMU综合误差方程，求解误差系数,建立陀螺和加速度计误差模型,标定完成,2
3、.1.4 IMU的误差标定与补偿,IMU的综合误差方程,角速度通道误差方程,输入：,输出：,X轴：,标定就是确定这些误差系数,传统标定方法：多位置静态标定方法,可标定全部33个误差系数，但精度不高、效率低,y,角速率标定法,传统标定方法：,精度较高，但仅标定9个误差系数,x,y,z,连续旋转,传统方法,7类 33个误差系数,IMU的无定向动静混合高精度标定方法,动态：旋转积分,静态：对称位置误差相消,动静混合,初始对准的必要性,2.1.5 SINS的静基座初始对准,为什么需要进行初始对准,积分运算必须知道 初始值！,粗对准与精对准,根据对准精度的要求，对准过程分为：,粗对准,精对准,要求尽快地
4、将平台调整到一个精度范围内，缩短对准时间是主要指标,在粗对准基础上进行，对准精度是主要指标,通常在精对准过程中要进行陀螺的测漂和定标，进一步补偿陀螺漂移率和标定刻度系数，以提高对准精度,平台，先水平（调平），后方位，使系统有较好的动态性能捷联：精确建立姿态矩阵 水平和方位对准同时（现代） 先水平后方位（经典）,初始对准方案,初始对准分为两大类：,频域法 或经典法,最优估计法 或卡尔曼滤波法,基于经典控制理论,基于现代控制理论,本课程研究的重点！,惯导系统 误差根源,加速度计偏置,陀螺漂移,随机误差,捷联惯导 为随机系统,抑制随机误差最优估计！,卡 尔 曼 滤 波 器,或,卡尔曼滤波,X 系统状
5、态向量,W 系统噪声向量,（1）采用KALMAN滤波进行初始对准，就是将平台误差角N，E，D从随机误差和随机干扰中估计出来，通过系统的校正使平台坐标系与导航坐标系对准；,（2）同时，尽可能估计出陀螺漂移和加速度计偏置；,（3）时间不长，因此陀螺漂移和加速度计偏置可看作常值；,初始对准的发展,初始对准的要求,初 始 对 准 的 发 展,新的滤波方法,可观测性分析和 可观测度研究,自适应滤波,H滤波,神经网络,非线性滤波,预测滤波,速度,精度,从根本提高对准的精度和速度,惯导误差方程,惯导误差方程,惯导系统的误差方程,正确反映惯导系统的误差特性，便于分析和应用！,角误差模型,角误差模型,可以证明两
6、种模型是等价的！,角误差模型和角误差模型,惯导系统的误差方程,平动误差方程,姿态误差方程,描述惯导系统误差特性的微分方程可分为：,：平台坐标系与真实地理坐标系之间的误差角, ：平台坐标系与计算地理坐标系之间的误差角,目前大多采用角误差模型和速度误差表达形式！,惯导系统的误差方程,（1）平动误差不会耦合到姿态误差方程中，特别便于动基座对准问题的分析和研究。,角误差模型+速度误差表达形式的优点：,（2）动角可通过位置误差和角得到： = + ,（3）静基座时，惯导所处地理位置可精确获得，且对准时间较短，可忽略位置误差，此时： = ,惯导系统的误差方程,角误差方程：,V、r和分别为速度、位置和姿态矢量
7、为地球自转角速度为导航坐标系相对惯性坐标系的角速度矢量是加速度计常值偏值，是陀螺常值漂移f是比力，g是重力矢量计算误差，是导航系相对地球转动速度矢量,误差模型可由下列3个基本方程表示：,.,.,.,惯导系统的误差方程,角误差方程：,在北东地坐标系中，有：,可得状态空间模型：,.,.,.,惯导系统的误差方程,静基座初始对准时，位置和垂直方向速度可准确知道 惯导系统的误差方程可简化为：,不完全为白噪声,扩充为系统状态变量,惯导系统的误差方程,最终可得角误差方程：,惯导系统的误差方程,角误差方程：,将上式微分：,角与角之间的关系：,惯导系统的误差方程,角误差方程：,在静基座条件下，最终可得：,惯导系
8、统的误差方程,静基座条件下速度误差方程：,速度误差定义为计算速度与真实速度之差,静基座条件下位置误差方程：,惯导系统的误差方程,最终可得，角误差方程：,不考虑的角误差方程可简化为：,卡尔曼滤波方程的建立,（1）系统方程,X 系统状态向量,W 系统噪声向量,其中，WVN。 WD 为零均值高斯白噪声，分别为加速度计误差和陀螺漂移的噪声成分,卡尔曼滤波方程的建立,（1）系统方程,系统转移矩阵,（2）量测方程,取两个水平速度误差VN 和VE为观测量,即：,Z=Z1， Z2=VN ，VE 为观测量H为观测矩阵=N ，E为观测方程的随机噪声状态矢量为零均值高斯白噪声,卡尔曼滤波方程的建立,（3）离散卡尔曼
9、滤波方程,或,卡尔曼滤波方程的建立,计算机仿真结果,仿真条件:陀螺常值漂移:0.02/h ；陀螺随机漂移:0.01/h ；加速度计常值偏置:100ug；加速度计随机误差:50ug；初始失准角N，E，D: 1惯导所处位置的地理纬度： L = 45 初始值的选取:X（0）均取为0；P（0）为粗对准后，位置、速度、姿态和惯性器件误差的方差Q 对应陀螺和加速度计随机误差的方差 ；R 对应量测随机误差的方差 ；,计算机仿真结果,计算机仿真结果,计算机仿真结果,分析: 收敛速度方面：N和E收敛较快，约20秒，D约5分钟以上 估计精度方面：N和E的稳态估计误差为20“，D 的稳态估计误差为6.48 陀螺漂移
10、的估计： N在15分钟以内可以估计出来D在虽然能勉强估计出来，但效果很差E估计不出来 加速度计偏置的估计： x ，y也估计不出来,计算机仿真结果,分析可知: N和E的估计精度由E和N决定,D的估计精度由E决定,静基座对准的可观测性分析,静基座初始对准数学模型：,静基座对准的可观测性分析,根据线性定常系统可观测性判定准则：,系统不完全可观测,7个状态可观测 3个状态不可观测,10个状态变量中哪3个不可观测,静基座对准的可观测性分析,利用奇异值分解来求秩，可仔细分析！,提高静基座对准精度和速度的方法,提高系统可观测度,快速对准方法,提高对准精度,提高对准速度,惯导误差模型的前5个方程如下:,KF,
11、提高静基座对准精度和速度的方法,提高静基座对准精度和速度的方法,由和 得:,又有:,所以稳态估计误差正好为:,又,提高静基座对准精度 和速度的方法,由可 得:,将,代入 :,由 式可得快速初始对准方法:,2.2 卫星导航系统,2.2.1 卫星导航系统基本原理无线电定位导航,空间星座,地面监控,用户设备,+,+,GPS 接收机,卫星监测站 主控站 信息注入站,24颗GPS卫星,2.2.2 卫星导航系统组成,无线电导航受区域限制,80年代开始发展卫星导航（将发射台放到卫星上）,绝对定位 相对定位,GPS 接收机天线,2.2.3 GPS的基本原理,2.2.4 GPS误差特性,时钟误差,星历误差,大气
12、层误差,电离层延时误差,多路径效应,随机性误差,动态滤波消除随机误差,2.2.5 GPS动态滤波的数学模型,载体运动模型,X为载体在x,y,z三个坐标轴上的位置、速度和加速度分量,2.2.5 GPS动态滤波的数学模型,将GPS经校正残留的随机误差，等效成时钟误差, 时钟偏置等效距离误差；, 时钟频漂的等效距离变化率误差；, 反相关时间；, 白噪声。,GPS误差模型,即：,2.2.5 GPS动态滤波的数学模型,GPS动态滤波的系统方程与量测方程如下：,2.2.6 GPS动态定位的滤波理论与方法,GPS动态定位最优滤波,自适应扩展卡尔曼滤波 改进的强跟踪卡尔曼滤波 ,建立机动载体合理、准确的运动数
13、学模型 采用先进的自适应算法,关键在于：,2.3 几种最优估计方法,什么是估计?,测量得出的数据,测量噪声,X(t)的估计,是Z(t)的函数,若为线性函数,则,称作X(t)的线性估计,解算,2.3.1 最优估计的基本概念,什么是预测? 什么是平滑?,当t=t1时,称为X(t)的估计；,设在t0，t1时间段内量测为Z， 待求状态为,当tt1时,称为X(t)的预测；,当tt1时,称为X(t)的平滑。,什么是最优估计？最小二乘估计？,则所得估计为最优估计！,若以量测估计的偏差的平方和达到最小为指标,则所得估计为最小二乘估计！,什么是最小方差估计？线性最小方差估计？,若以状态估计的均方误差集平均达到最
14、小为指标,则所得估计为最小方差估计！,若 又是X的线性估计，,则 为X的线性最小方差估计,2.3.2 最小二乘估计,一般最小二乘,n维,无法直接测量,确定性 常量向量,测量值Z为 的线性组合，记第i次测量Zi为:,mi维向量,量测矩阵,随机量测噪声,若共测量r次，即：,由上述诸式可得描述r次测量的量测方程：,若 又是X的线性估计，,最小二乘估计的指标函数,线性估计,最小二乘估计的性质,假设：量测噪声V是均值噪声，方差为R的随机向量,（1）最小二乘估计是无偏估计,（2）最小二乘的均方误差阵,2.3.3 加权最小二乘估计,一般最小二乘精度不高,Z1,Z2,Zm,普通最小 二乘估计,加权最小,W1,
15、W2,Wm,加权最小二乘的指标函数,若WI，则,普通最小二乘 指标函数,通常W取为对称阵，即WTW，则加权最小二乘估计为：,加权最小二乘的估计误差,量测 误差,若V的均值为零，方差阵为R，则：,加权最小二乘估计也是无偏估计！,估计的均方差为：,加权最小二乘的估计误差,若WR1，则加权最小二乘估计为：,马尔可夫 估计,马尔可夫估计的均方误差为：,比其他加权最小二乘估计均方差都小最优的加权最小二乘估计,2.3.4 递推最小二乘估计,量测信息越多，只要处理得当，最小二乘估计的均方误差越小,采用批处理实现的最小二乘算法，需存储所有量测值,实时性递推最小二乘估计从每次获得的量测值重提取出被估计量的信息，用于修正上一步所得的估计。获得量测的次数越多，修正的次数也越多，估计精度也越高。,设X为确定性常值向量，前k次观测积累量测为量测方程为：,式中：,Zi的第i次量测方程为：,则前k1次的量测方程为：,式中：,Zk+1为第k1次量测值，量测方程为,由前k次量测值确定的加权最小二乘估计为：,由前k+1次量测值确定的加权最小二乘估计为：,推导得：,谢 谢,
