Sigma点滤波在GPS_INS导航系统中应用及仿真
GNSS／INS组合导航在飞行校验系统中的应用
（ Ｉ ＭＵ） ｙｏａ ｄａｃｌ ｏ ｔ ａｕｅ ｎｓ ｇ ｒ ｃｅ ｒｍｅ ｒ ｎ ｅ ｅ ｍｅｓｒｍｅｔ．
Ｋ ｅ ｒ ｓ Ｓ Ａ ｅ ｈ ｏｏ ｙ Ｆ ｉｈ ｓ ｅｔ ｎ Ｇ ＳＩ ｔｇａｅ ｖｇ ｔ ｎ ｙ ｗｏ ｄ ： Ｐ Ｎ Ｔ ｃ ｌｇ ； ｌ ｔ ｎ ｐ ｃ ｏ ； ＮＳ ／ ｎ ｇ Ｉ ｉ ＮＳＩ ｅ ｒｔｄ ｎ Ｎａ ｉａ ｏ ｉ
ｉｓ ｅ ｔ ｎ ｓ ｓｅ ｃ ｌ ｅ ｓ ｒ ｅ ｉ ｌｍ ｅ ｔｔｏ ｆｃ ｎｉｕ ｕ ．ｓａ ｌ ｎ ｅｉｂｅ ｐ ｓｔ ｎ ａ ｄ ａｔＬ ｅ ｄ ｔ ｎ ｅ ｏ ｓｏ Ｓ ｎ ｐ ｃｉ ｙ ｔｍ ａｌ ｎ ｕ ｅｔ ｍｐｅ ｎ ａｉｎ ｏ ｏ ｔ ｏ ｓ ｔｂｅ ａ ｄ ｒｌ ｌ ｏ ｉｏ ｎ ｔｔＬ ａａｕ ｄ ｒｌｓ ｆＧＰ ｏ ｈ ｎ ａ ｉ ｉ ｄ
的误 差 ，其 中由定位 系 统所提 供 的空 间基 准坐 标 是
否精确直接决定 了飞行校验的精度和可靠性。通常
的 飞行校 验 系统 （ Ｉ ） 用伪 距 差分 Ｇ Ｓ（ ＦＳ 采 Ｐ ＤＧＰ ， Ｓ
评估各种导航、雷达、通信等设备的空间信号的质
Ｄｉｅｅｔ ｌ Ｓ ｆ ｒｎｉ ）或 实 时载波 相位 差 分 Ｇ ＳＲ Ｋ， ａＧＰ Ｐ （Ｔ
ｍｏ ｅａ ｃ ａｅａ ｄｒｌ ｂｅｉ ｃｕ ａ ｙ ｐ ｒｏ ａ ｃ ｄｒｌ ｂｌ ａ ｉ ｅ ｒ ｃ ｕ ｔ ｅｉ ｌ ａ ｃ ｃ ， ｅ ｒ ｎ ｅａ ｅｉ ｉｔ ｔ ｎｅｔ ｒＧＮＳ ｒＩ Ｓｈ ｓ Ｔ ｅａ ｐｉａｉｎｏ Ｐ ｒ ｎ ａ ｎ ｒ ｆ ｍ ｎ ａ ｉ ｈ ｙ ｈ Ｓ ｏ ａ ． ｈ ｐ ｌ ｔ ｆＳ ＡＮ ｎｆｉｈ Ｎ ｃ ｏ ｏ ｇｔ ｌ
即为 Ｓ Ａ 技 术 。实物 图如 图 １ 示 。 ＰＮ 所
基于Sigma点Kalman滤波的车载导航系统状态估计
．
ａ ｄ Ａｐ ｌ ａ ｏ ｓ，０ ７，３（ ０ ： ３ － ３ ． ｎ ｐｉ ｔ ｎ ２ ０ ４ １ ） ２ ２ ２ ５ ｃｉ
Ａｂ ｔａ ｔ ｎ ｖｅ ｆｔｅ ｄ ｆｃｓｗｈ ｎ Ｅ ｔｎ ｅ ｌ ｎ Ｆｈ ｒＥＫＦ ｓ ｅ ｌｙ ｄ ｉ ｈ ｏ ｌ ｅ ｒｌｎ ｅ ｉ１ ｙｔｍ ｔ ｉ ｐ ｐｅ ｓｒ ｃ ：Ｉ ｉｗ ｏ ｈ ｅｅ ｔ ｅ ｘｅ ｄ ｄ Ｋａｍａ ｉｅ （ ）ｉ ｍｐｏ ｅ ｎ ｔｅ ｎ ｎｉ ａ ａ ｄ ｖ ｈｃｅ ｓ ｓ ｎ ｅ ｈｓ ａ ｒ
高阶无迹卡尔曼滤波算法在飞机定位中的应用
高阶无迹卡尔曼滤波算法在飞机定位中的应用刘家学;林松岩【摘要】无迹卡尔曼滤波算法（UKF）在飞机定位和跟踪的过程中精度不够，原因在于误差变量的偏度和峰态在坐标转换过程中对其分布影响很大。
为了解决这一问题，将高阶无迹卡尔曼滤波算法应用到 QAR 数据中。
首先，根据高阶 UT 变换，选取一组样本点（sigma 点）表征 k 时刻最优估计值前四阶矩的分布特征，通过传递得到 k ＋1时刻一步预测值的先验概率分布。
然后以观测数据作为量测值，带入滤波算法得到 k ＋1时刻飞机状态的最优估计值。
最后根据计算机产生的模拟噪声数据和真实的 QAR 数据实现飞机定位的仿真验证。
从仿真结果看，高阶无迹卡尔曼滤波算法比无迹卡尔曼滤波精度更高，误差更小，对 QAR 数据中其他类型的数据形式有一定的借鉴意义。
%The cause of inadequate accuracy of unscented Kalman filter (UKF)in localising and tracing an airplane is due to the very big influence of skewness and kurtosis of error variables on the distribution of coordinate during its transformation process.In order to solve the problem,we applied high-order UKF algorithm to quick access recorder (QAR)data.First,according to high-order unscented transformation (UT)we chose a set of sample points (sigma points)to characterise the distribution feature of the first four moments of optimal estimating valueat time k ,and obtained the priori probability distribution of one-step prediction value at time k +1 through transferring.Then we took the observation data as the measured value,and brought in the filtering algorithm to get the optimal estimation value of airplane state at time k+1.At last,according to computer-generated simulative noise data andactual QAR data we achieved the simulated validation of airplane localisation.From the simulation result it is aware that the high-order UKF algorithm has higher accuracy and less error than UKF,this has certain reference significance to the data form of other types in QAR data.【期刊名称】《计算机应用与软件》【年(卷),期】2016(033)005【总页数】5页(P256-259,264)【关键词】QAR 数据;高阶 UT 变换;高阶无迹卡尔曼滤波【作者】刘家学;林松岩【作者单位】中国民航大学航空自动化学院天津 300300;中国民航大学航空自动化学院天津 300300【正文语种】中文【中图分类】TP391对民航客机进行精确的定位和跟踪尤为重要，但是在以经度、纬度和高度为参考的坐标系中无法有效表示飞机的运动过程，所以常常转化到WGS-84坐标系中进行计算，再通过经度和纬度表示出来。
8联邦滤波
第8章联邦滤波和自适应滤波第8章联邦滤波和自适应滤波在组合导航中的应用8.1联邦卡尔曼滤波组合导航系统可提高系统的任务可靠性和容错性能。
因为组合导航中有余度的导航信息，如组合适当，则可利用余度信息检测出某导航子系统的故障，将此失效的子系统隔离掉，并将剩下的正常的子系统重新组合（系统重构），就可继续完成导航任务。
组合导航系统还可协助惯导系统进行空中对正和校准，从而提高飞机或其他载体的快速反应能力。
联邦卡尔曼滤波理论是美国学者 Carlson于1998年提出的一种特殊形式的分布式卡尔曼滤波方法。
它由若干个子滤波器和一个主滤波器组成，是一个具有分块估计、两步级联的分散化滤波方法，关键在于它采用信息分配原理。
它需要向各子滤波器分配动态信息，这些信息包括两大类：状态方程的信息和观测方程的信息。
8.1.1联邦卡尔曼滤波器结构运动方程的信息量与状态方程中过程噪声的协方差阵成反比，过程噪声越弱，状态方程就越精确。
因此，状态方程的信息量可以用过程噪声协方差阵的逆 Q1来表示。
此外，状态估计的信息量可用状态估计协方差阵的逆P1表示，测量方程的信息量可用测量噪声协方差阵的逆 R1表示。
如果把局部滤波器i的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为召、Q i、R,i 1,2, ,n，主滤波器的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为）?m、Q m、P m,假定按以下规则将整体信息分配至各局部滤波器，即P1）? p1 1）?1 p21）?2叮冷PrJX m 斤刃（8.1）Q 1Q11Q21Q n1Q m1Q i1i Q 1（ 8.2）P 1P11P21P n1P m1P i 1i P 1（ 8.3）其中，i是信息分配系数，必须满足下列条件:在设计联邦卡尔曼滤波器时，信息分配系数的确定至关重要，不同的值会有不同的滤波器结构和特性（容错性、最优性、计算量等）。
令i 1/ i （i 1,2……N,m ）,则它的的几种主要结构可简要地表达如下：（1）第一类结构（m i =1/ （ N + D ，有重置），如图8.1所示图8.1 联邦滤波器第一类结构这类结构的特点是：信息在主滤波器和各子滤波器之间平均分配。
gvins原理
gvins原理GVINS原理GVINS是一种基于惯性导航系统（INS）和全局导航卫星系统（GNSS）的组合导航算法，它可以在没有地面参考站的情况下实现高精度的位置、速度和姿态估计。
本文将详细介绍GVINS原理。
一、GVINS概述1.1 INS和GNSS惯性导航系统（INS）是一种利用加速度计和陀螺仪等传感器测量飞行器运动状态的设备，可以实现高精度的位置、速度和姿态估计，但由于误差积累问题，长时间使用会导致位置漂移。
全球卫星定位系统（GNSS）是一种利用卫星信号测量飞行器位置、速度和时间的设备，可以提供较高的位置精度，但在高纬度地区或遮挡区域会受到干扰。
1.2 组合导航组合导航是将多个传感器的信息进行融合以提高导航精度的方法。
INS 和GNSS可以通过组合来弥补彼此的不足，并提高整个系统的精度。
1.3 GVINSGVINS是一种基于组合导航算法实现高精度定位、速度和姿态估计的方法。
它通过将INS和GNSS的信息进行融合，利用卡尔曼滤波算法进行状态估计和误差校正，从而实现高精度的导航。
二、GVINS原理2.1 系统模型GVINS系统模型包括状态方程和观测方程。
状态方程描述了系统的动态行为，包括位置、速度、姿态和传感器误差等；观测方程描述了系统的输出，即传感器测量值。
GVINS系统模型可以表示为：x(k+1)=f(x(k),u(k))+w(k)z(k)=h(x(k))+v(k)其中，x表示状态向量，包括位置、速度、姿态和传感器误差；u表示控制向量，包括飞行器加速度和角速度；w和v分别为过程噪声和观测噪声。
2.2 状态估计GVINS利用卡尔曼滤波算法对系统状态进行估计。
卡尔曼滤波算法是一种递归最小二乘估计方法，可以有效地处理线性高斯问题。
GVINS 中使用扩展卡尔曼滤波（EKF）对非线性问题进行处理。
EKF基于一阶泰勒展开式对非线性函数进行线性化，并使用高斯分布来描述状态和误差的不确定性。
EKF包括预测和更新两个步骤。
惯性导航系统：导航不再非卫星不可
FPGA作为主控制器进行实时数据采集和通信，以NIOSII软核处理器进行 惯性传感器的数据采集和处理。描述了系统的总体结构和利用六位置法 争转台实验对惯性器件进行了补偿算法的研究。通过六位置法和转台实 验的测试，对应原理样机的实测数据与补偿后的结果，验征了惯
惯性导航技术的理论技术是什么？ 惯性导航系统的工作机理是建立在牛顿经典力学的基础上的。牛顿定律告诉
人们：一个物体如果没有外力作用，将保持静止或匀速直线运动；而且 ，物体的加速度正比于作用在物体上的外力。如果能够测量得到加速度 ，那么通过加速度对时间的连续数学积分就可计算得到物体的速度和位 置的变化。
惯性导航技术的特点是什么？ 不同于其他类型的导航系统，惯性导航系统是完全自主的，它既不向外部发
射信号，也不从外部接收信号。惯性导航系统必须精确地知道在导航起 始时运载体的位置，惯性测量值用来估算在启动之后所发生的位置变化
除了改进惯导系统中的陀螺仪等设备，还有没有其它办法解决惯性导航长时 间工作的精度问题？
惯性导航统有什么缺点？ 其缺点是：1、由于导航信息经过积分而产生，定位误差随时间而增大，长
期精度差；2、每次使用之前需要较长的初始对准时间；3、设备的价格 较昂贵；4、不能给出时间信息。
惯性导航系统的工作原理是什么？ 惯性导航系统属于推算导航方式，即从一已知点的位置根据连续测得的运动
体航向角和速度推算出其下一点的位置，因而可连续测出运动体的当前 位置。惯性导航系统中的陀螺仪用来形成一个导航坐标系，使加速度计 的测量轴稳定在该坐标系中，并给出航向和姿态角；加速度计用来测量 运动体的加速度，经过对时间的一次积分得到速度，速度再经过对时间 的一次积分即可得到距离
GPS卫星信号模拟器中频信号处理与实现
摇 基金项目: 国家自然科学基金资助项目(60027001) ; 北京市科技新星计划资助项目 摇 作者简介: 李保柱(1967-) , 男, 江苏连云港人, 博士生,leebz285712@ tom. com.
812
号
[2]
北 京 航 空 航 天 大 学 学 报摇 摇 摇 摇 摇 摇 摇 摇 摇 摇 摇
好的抗混叠、 抗镜像功能, 运算效率高、 节省硬件 资源. 一 般 单 速 率 数 字 FIR ( Finite Impulse Re鄄 sponse) 滤波器抽头数由下式决定: y( k) = a( n) x( k - n) 摇 移 n=0
N -1
利用内插方法实现的数字低通滤波器具有良
钟周期数 L 值为 4,取样速率为 37 . 5 MHz. 达到抗 混叠、 抗镜像功能. 滤波器的并行处理位数越多, 需要利用的 FPGA 资源越多. 滤波器的取样速率 取合适的参数,达到数字滤波目的. 与系统时钟频率 f clk 、 输入取样数据比特位宽 B 、
经低通数字滤波器 h ( n2 T2 ) 滤去镜像频谱,
^
2 基带数字信号到中频模拟信号实现
个采样点插入 I -1 个零点,得到内插信号. 应用内 插理论能够在较低的采样速率产生较高频率的中 频信号,降低整个系统的基带处理频率,降低对微 处理器和 FPGA( Field Programmable Gate Arrays) 运算速度的要求,提高时域分辨率. 对数字信号进 行内插、数字滤波,D / A( Digital / Analog) 变换和模 拟滤波, 实现中频信号的频率变换, 得到 GPS 模 拟器中频模拟信号. GPS 模拟器基带数字信号变 换为中频模拟信号实现原理见图 2. 对基带数字信号进行 I 倍零内插, 即每隔 1
GNSS INS 组合导航原理与应用(完整版)
、组合导航系统（续）
卫星组合导航的性能优势
➢ 由于多星座提高了卫星星座的几何结构，增强了可用性（）； ➢ 全部建成后，卫星覆盖率将极大增强（星空璀璨——颗卫星以上），提高导
航
技
成本昂贵，不为我国所有
术 缺点 不能输出姿态信息
输出不连续
25
（）各种导航技术的特点？
天文导航——古老而又年轻的导航技术
天文导航是一种利用光恒星）信息
进行载体位置计算的定位导航方法。
完全天文定位导航 基于航天器轨道动力学方程的定位导航
采用组合导航技术的系统称为组合导航系统 参与组合的各导航系统称为子系统。
二、 组合导航的基本方法 回路反馈法 采用经典的回路控制方法，抑制系统误差， 并使各系统间实现性能互补；
最优估计法 采用卡尔曼滤波，从概率统计最优的角度
估计出系统误差并消除之。
连续输出位置、速度、姿态
惯性导航技术现状
美国实验室对当前陀螺仪发展现状分析
惯性导航技术发展趋势
美国实验室对年陀螺仪发展趋势的预测
（）各种导航技术的特点？ 无线电导航技术——基本原理
22
（）各种导航技术的特点？
无线电导航技术——卫星导航技术
无线电导航受区域限制 年代开始发展卫星导航（将发射台放到卫星上）
26
（）各种导航技术的特点？
天文导航——基本原理
舰船天文导航基本原理
即通过观测不同天体或不同时刻观测同一天体，
以各天体投影点为圆心，各观测天体高度为半径画天
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
收稿日期:2008 04 02;修回日期:2008 06 17.基金项目:国防预先研究支撑课题资助(619010702).作者简介:周 磊(1983-),男,在读硕士,主要研究方向为惯性导航、GPS 软件接收机和数据融合算法.S i g m a 点滤波在GPS /INS 导航系统中应用及仿真周 磊1, 徐德民1, 龚 诚2(1 西北工业大学航海学院,陕西西安,710072;2 中国航空无线电电子研究所,上海200233)摘 要:为了弥补现有组合导航算法的不足,提出了一种新的GPS 量测数据和惯性导航系统(I NS )数据的融合算法。
近几十年,组合导航系统中最广泛使用的算法扩展卡尔曼滤波(EKF )是次优滤波器,因为它把非线性模型简化为1阶线性模型,且假设该系统由高斯白噪声驱动,这种简化会导致误差的扩大。
而Sig m a 点卡尔曼滤波器可以克服这些缺点。
S i gm a 点卡尔曼滤波器无需将系统动力学模型线性化,且在Sig m a 点卡尔曼滤波器中状态的分布采用选择的样本点集合来表示。
通过这些样本点可以完全获得高斯随机变量真实的均值和方差,并且高斯随机变量在非线性系统传播时,其均值和方差可以至少精确到2阶。
仿真结果表明,Sig m a 点卡尔曼滤波器在GPS /I N S 导航系统中表现性能良好。
关键词:惯性导航系统;G PS /I N S 组合导航;S i gma 点卡尔曼滤波;非线性系统;仿真中图分类号:TN967 2;P228 4 文献标识码:A 文章编号:1673 1948(2009)05 0048 05Applicati on of Si g ma Po i nt F ilteri ng to GPS /INSIntegrated N avigation Syste m w ith Si m ul ati onZ HOU L ei 1, X U D e M in 1, GONG Cheng2(1.Co lleg e o fM ar i ne Eng i neer i ng ,N orth w este rn P olytechn i ca lU n i versity ,X i an 710072,Ch i na ;2 Ch i nese A eronautica l R ad i o E l ectronics R esea rch Instit ute ,Shangha i 200233,China)Abstrac t :A new f usi on algorith m of the m easure m ents from g l obal position i ng system (GPS)and ine rtia l nav i ga ti on sy stem (INS)is presented t o com pensate the w eaknesses of current integ ra ted nav i ga tion system .T he ex tended K a l m an filteri ng (EK F),an a l gor ith m co mmonly used i n i ntegrated nav igati on syste m i s a subopti m a l one i n a sense because it si m plifi es t he nonli near dyna m ic syste m model t o a first o rder li near model and assu m es that the li nearized one is driven by G aussian w hite no ise .T his s i m p lifica ti on l eads to questi onab l e resu lts i n certa i n scenar i os .One of t he i m proved versi ons of E K F is s i g m a pointK al m an filter i ng (SP KF ).SPKF needn t assu m e a li nea rized dyna m i c m ode ,l and uses samp l es set to represent the state d i str i buti on ,lead i ng itself an adequa te candi date for nav i ga ti on app licati ons .One o f the good features o f SPKF i s that the states m ean and covar i ance can be eas il y obta i ned .F or G aussian rando m var i ab l es ,the m ean and covar i ance can accurate l y approx i m a te to at least second o rder i n any nonli near syste m.S i m u l a ti on resu lts show that SPKF wo rks w ell in G PS /I NS integ ra ted nav i g ati on syste m.K ey words :i nertial nav i gation syste m (I N S);GPS /I N S i n teg ra ted nav i gation ;si gm a po i nt K a l m an filte ri ng(SPK F);non li near sys te m;si m u lati on0 引言以惯导导航系统(Inertial Navigation Syste m,I N S)和全球定位系统(G l o ba l Positi o ning Syste m,GPS)构造的组合导航系统是导航中最主要的组合方式。
由于2个系统各有其优缺点,而在误差传播特性上又是互补的,前者短期稳定性好,数据输出频率高,但长期稳定性差;而后者正好相反。
第17卷第5期2009年10月 鱼 雷 技 术TORPEDO TECHNO LOGYV o.l 17N o .5O ct .2009因此,可以利用GPS的信息限制I NS误差的积累,当GPS接收机丢失卫星信号时,可以利用I N S 的输出信息辅助接收机快速地捕获和跟踪卫星信号。
这种组合不仅能有效地提高系统精度,同时极大地降低了系统的成本,因此越来越广泛地应用于航空、航天和航海领域。
依靠加速度计和陀螺仪,I N S可以输出完整持续的导航信息,包括位置、速度和姿态信息,随着时间累积的误差可以由GPS提供的数据来修正。
在大部分系统中,使用GPS的位置和速度信息,这种组合称之为松耦合,本文采用的即是这种组合方式。
最近几十年,在组合导航系统中最广泛使用的算法是扩展卡尔曼滤波(Ex tended K al m an Filteri n g,EKF)[1 2]。
而在S i g m e点卡尔曼滤波(S ig m a Point Kal m an F iltering,SPKF)中,一系列Sig m a点通过非线性方程来转移,且可以精确到2阶。
权重参数和刻度参数的选择,决定了SPKF的类型,如无偏卡尔曼滤波[3 4]。
1 组合导航系统的动力学方程描述飞行器姿态运动学的4元数微分方程[5]q(t)=12[ b ab(t)]q(t)(1)其中, b a b为飞行器旋转角速度在机体坐标系的表示。
[ b ab]=0- b abx- b aby- babzb a bx0 b abz- b a byb a by- b abz0 b abxbabzba by-babx0(2)飞行器线速度向量的微分方程[5] V NV EV D=f Nf Ef D+0-(+2 ie)sin++(+2 ie)si n0+(+2 ie)cos- -(+2 ie)cos0V NV EV D+g(3) 位置向量的微分方程[5]=V NR e(1-2e+3e si n2)+(h a)!(h i)1-!=V ER e(1+e2si n2)+(h a)!(h i)1-!cosh i=-V D(4)式中:, 和h!!!分别为飞行器的经度、纬度和高度;R e!!!地球赤道半径;e!!!地球离心率;h i!!!惯性系统计算的高度;h a!!!气压高度计测量的高度;!!!!加权因子。
GPS接收机钟差模型包括钟差c∀t和钟差漂移c∀f,它们均由高斯白噪声w c∀t和w c∀f驱动[4]。
c∀ tc∀f=0100c∀tc∀f+w c∀tw c∀f(5)显然,该系统的动力学模型是非线性的。
由于使用的是低成本的微型机电系统惯性测量装置(M icro E lectro m ec han ica l Syste m Inertial M easurem ent Un i,t ME M S I M U),速度微分方程中的Coriolis力项可以忽略。
陀螺仪感受不到地球自传,在4元数微分方程中与地球自转有关的项可以忽略。
并且由纬度和高度引起的重力的变化可以忽略。
尽管忽略了次要因素,该系统仍然是非线性系统。
2 Sig m a点卡尔曼滤波SPKF不需要对非线性动力学方程和量测方程采用Jacob i a n矩阵线性化,而是采用一系列与原随机向量具有相同均值和协方差的Sig m a点。
这些S ig m a点通过非线性模型转移,同时由这些点计算随机状态向量的均值和协方差。
2 1 算法设计记增广状态向量[3]X^a=E[X a]=[X T w T v T]T(6)式中:X!!!标准卡尔曼滤波状态方程[5]中的状态向量;w,v!!!分别为系统的过程噪声和测量噪声向量。
增广状态向量X^a估计误差的协方差阵P a=E(a-X a)(a-X a)T49第5期 周 磊,等:S i gm a点滤波在GPS/INS导航系统中应用及仿真=P x 000Q w 00R v(7)式中:P x ,Q w ,R v !!!分别表示标准卡尔曼滤波中的协方差阵、系统噪声阵和量测噪声阵。
假设采用长度为2L +1的S i g m a 点列[3 4],这里L 是增广向量的维数,第i 个sig m a 点记为X ai,k ,下标k 表示第k 时间。