ISR-CDKF在SINS大方位失准角初始对准中的应用

贾鹤鸣, 宋文龙, 牟宏伟, 车延庭

(1. 东北林业大学 机电工程学院, 哈尔滨 150040； 2. 中国运载火箭技术研究院, 北京100076；3. 哈尔滨工程大学 自动化学院, 哈尔滨 150001)

0 引 言

初始对准是捷联惯性导航系统(SINS: Strapdown Inertial Navigation System)的一项关键技术[1-3]。随着对惯性技术要求的不断提高, 大方位失准角方法的提出使得对非线性滤波算法的研究具有十分重要的意义。

近年来, 中心差分卡尔曼滤波(CDKF: Central Difference Kalman Filter)在非线性估计领域中得到了广泛的应用[4,5]。该算法虽然克服了扩展卡尔曼滤波(EKF: Extended Kalman Filter)由于线性化误差而导致滤波器精度降低和需要计算雅可比矩阵(Jacobian)的缺点[6], 但是CDKF存在计算量大以及算法不稳定等缺点[7]。针对以上不足, 笔者提出了迭代测量更新的平方根中心差分卡尔曼滤波(ISR-CDKF: Iterative Square Root Central Difference Kalman Filter)算法。该算法利用协方差平方根代替协方差参加递推运算, 保证了滤波的数值稳定性。同时, 在此基础上改进了量测更新方法, 重复利用观测信息, 提高了非线性估计精度。

笔者将ISR-CDKF应用于SINS大方位失准角初始对准中, 通过仿真比较, 进一步表明了ISR-CDKF不仅可提高系统的精度, 而且保证了数值稳定性, 验证了该算法的可行性和优越性。

1 大方位失准角初始对准误差模型

(1)

其中φx、φy、φz为3个欧拉角； 当φx,φy很小时, cosφx=cosφy=1, sinφx≈φx, sinφy≈φy, 则

(2)

1.1 姿态角误差方程

(3)

忽略二阶小量, 式(3)化简可得

(4)

(5)

(6)

(7)

(8)

1.2 速度误差方程

SINS的理论速度微分方程为

(9)

SINS导航解算的速度微分方程为

(10)

(11)

1.3 捷联惯导系统非线性初始对准模型

假设陀螺仪的测量误差为常值漂移εb； 加速度计的测量误差为常值零偏b； 在静基座初始对准条件下,其中R为地球半径,L为纬度, 忽略重力误差项δgt以及不考虑δvz, 则由式(8)和式(11)得到系统状态方程为

(12)

令状态向量

陀螺和加速度计的零均值高斯白色噪声向量

以SINS两水平速度误差Z=δVt作为观测量, 建立非线性对准模型如下

(13)

其中f(X)的具体表达式参考式(12),G为10×5维的矩阵系数,G(1,1)=G(2,2)=G(3,3)=G(4,4)=G(5,5)=1, 量测矩阵H=[02×3∶I2×2∶02×5],V为量测噪声。

2 迭代测量更新的平方根中心差分卡尔曼滤波

2.1 中心差分变换

中心差分变换是一种基于插值理论的非线性变换方法, 是CDKF算法的基础。CDKF借助于中心差分变换, 利用sterling插值公式, 用多项式逼近非线性方程的导数来避免复杂的求导运算, 它采用中心差分代替Tailor展开中的一阶和二阶导数[8,9]。

y=f(x)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

其中sxi表示cholesky分解中sx的第i列。

2.2 平方根U变换

U变换是一种通过设置Sigma样点和相应权值计算随机变量经非线性变换后的统计特性的方法[11-15]。

y=g(x)

(24)

利用这些样点通过非线性变换可得到的新Sigma样点为

y=g(χi)i=0,1,2,…,2L

(25)

其中y的均值和方差分别为

(26)

(27)

通常非负定的矩阵Py, 在受到计算误差等因素干扰后, 会变得不对称或负定, 从而影响滤波的收敛性和稳定性, 进而导致滤波器的发散[16-18]。针对以上问题, 可将Py分解为

(28)

Py的平方根矩阵Sy可通过Cholesky分解计算得到, 而它在滤波更新算法中的导出可通过QR分解实现[5]。在滤波过程中, 用Sy代替Py参加递推运算, 可保证协方差阵的非负定性, 从而实现滤波的有效性。

2.3 迭代测量更新的平方根CDKF算法(ISR-CDKF)

ISR-CDKF算法具体步骤如下：

1) 初始化

(29)

2) 确定权值

(30)

3) 计算时间更新所需的sigma点集

(31)

4) 时间更新

(32)

(33)

(34)

5) 计算测量更新所需的sigma点

(35)

6) 量测更新

(36)

forj=0 ∶n

end

(45)

(46)

3 仿真研究

3.1 仿真条件

假设系统的状态x的初始状态x(0)=0； 初始水平失准角φx=φy=0.6°, 方位失准角φz=10°； 陀螺常值漂移为0.02 (°)/h, 随机漂移为0.01 (°)/h； 加速度计零偏为1×10-4g, 随机偏差为0.5×10-4g； 速度测量误差为0.1 m/s； 当地地理纬度为45.779 6°。

根据上述仿真条件, 分别利用EKF、 CDKF和ISR-CDKF滤波算法, 对捷联惯导系统大方位失准角初始对准非线性误差模型进行滤波仿真, 仿真结果如图1和图2所示。

图1 北向失准角误差曲线 图2 天向失准角估计误差曲线

3.2 仿真结果分析

由于水平失准角(东向和北向)的估计结果相差无几, 因此这里只给出北向的情况。从图1可看出, 对水平失准角的估计, EKF、 CDKF和ISR-CDKF3种滤波算法的结果基本一致, 收敛速度均较快, 都能得到较高的估计精度。从图2可看出, 对大方位失准角的估计, CDKF和ISR-CDKF在收敛速度和估计精度上, 都得到了比EKF更好的效果。同时, ISR-CDKF与CDKF相比具有更高的滤波精度, 计算量小, 提高了算法的数值稳定性和运算效率。

4 结 论

笔者提出了一种基于迭代测量更新的平方根CDKF, 该算法通过迭代状态估计值改善非线性近似精度, 从而提高滤波精度。ISR-CDKF不仅具有CDKF无需计算雅可比矩阵的优点, 而且更加易于实现。ISR-CDKF通过对协方差平方根进行递推更新, 避免了协方差矩阵负定的情况, 提高了算法的数值稳定性, 而且比CDKF减小了计算量。仿真结果充分证实了其在大方位失准角初始对准中的可行性与优越性, 为实际应用提供了理论依据和计算方法。

参考文献：

[1]刘国海, 李康吉. 基于PDA的GPS定位精度提高方法 [J]. 江苏大学学报： 自然科学版. 2005, 26(5)： 448-452.

LIU Guo-hai, LI Kang-ji. Improving GPS Positioning Accuracy Based on PDA [J]. Journal of Jiangsu University: National Science Edition, 2005, 26:(5): 448-452.

[2]NORGAARD M, POUISEN NK, RAVN O. Advances in Derivative-Free State Estimation for Nonlinear Systems [R]. Tech Rep IMM-REP-1998-15: Tech Univ of Denmark, 2000.

[3]NORGAARD M, POUISEN N K, RAVN O. New Developments in State Estimation for Nonlinear System [J]. Automatic, 2000, 36(11)： 1627-1638.

[4]JULIER S J, UHLMANN J K. Unscented Filtering and Nonlinear Estimation [J]. Proceedings of IEEE, 2004, 92(3): 401-422.

[5]张贤达. 矩阵分析与应用 [M]. 北京: 清华大学出版社, 2004.

ZHANG Xian-da. Analysis and Applications [M]. Beijing: Tsinghua Press, 2004.

[6]肖嵩. 无线信道中的联合信源信道编码研究 [D]. 西安: 西安电子科技大学综合业务网国家重点实验室, 2004.

XIAO Song. Redearches on Joint Source Channel Coding in Wireless Channel [D]. Xi’an: The State Key Laboratoy of Integrated Services Networks, Xidian University, 2004.

[7]KAZUFUMI ITO, KAIGI XIONG. Gaussian Filters for Nonlinear Filtering Problems [J]. IEEE Transactions on Automatic Control, 2000, 45(5): 910-927.

[8]VAN DER MERWE R. Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models [D]. OGI School of Science & Engineering, University of Oregon Health & Science, 2004: 62-74.

[9]STANKOVIC J A, RAMAMRITHAM K, SPURI M. Deadline Scheduling for Real-Time Systems: EDF and Related Algorithms [M]. USA: Kluwer Academic Publishers, 1998.

[10]王海勃, 陈红林, 韩惠珍. CDKF在GPS/SINS组合导航系统非线性模型中的应用 [J]. 现在电子技术, 2011, 34(11)： 19-23.

WANG Hai-bo, CHEN Hong-lin, HAN Hui-zhen. Application of CDKF Method in GPS/SINS Integrated Navigation Systems Nonlinear Model [J]. Modern Electronics Technique, 2011, 34(11)： 19-23.

[11]王小旭, 赵琳, 夏全喜, 等. 基于Unscented变换的强跟踪滤波器 [J]. 控制与决策, 2010, 25(7)： 1063-1068.

WANG Xiao-xu, ZHAO Lin, XIA Quan-xi, et al. Strong Tracking Filter Based on Unscented Transformation [J]. Control and Decision, 2010, 25(7): 1063-1068.

[12]归庆明, 许阿裴, 韩松辉. 分步抗差自适应滤波及其在GPS动态导航中的应用 [J]. 武汉大学学报: 信息科学版, 2009, 34(6): 719-723.

GUI Qing-ming, XU A-pei, HAN Song-hui. A Stepped Robust and Adaptive Filtering and Its Applications in GPS Kinematic Navigation [J]. Geomatics and Information Science of Wuhan University, 2009, 34(6): 719-723.

[13]高为广, 何海波, 陈金平. 自适应UKF算法及其在GPS/INS组合导航中的应用 [J]. 北京理工大学学报, 2009, 34(6): 719-723.

GAO Wei-guang, HE Hai-bo, CHEN Jin-ping. An Adaptive UKF Algorithm and Its Application for GPS/INS Integrated Avigation System [J]. Transactions of Beijing Institute of Technology, 2009, 34(16): 719-723.

[14]LIU G, ZHAO N, LIU C A. RTT Estimation Based on Adaptive Filtering [C]//Proceedings of the 1st International Conference on Information Science and Engineering. Nanjing, China, [s.n.]. 2009: 1842-1846.

[15]孙枫, 唐李军. Cubature粒子滤波 [J]. 系统工程与电子技术, 2011, 33(11): 2554-2557.

SUN Feng, TANG Li-jun. Cubature Particle Filter [J]. Systems Engineering and Electronic, 2011, 33(11): 2554-2557.

[16]王虎, 王解先, 白贵霞, 等. 改进的渐消卡尔曼滤波在GPS动态定位中的应用 [J]. 同济大学学报: 自然科学版, 2011, 39(1): 124-128.

WANG Hu, WANG Jie-xian, BAI Gui-xia, et al. An Improved Fading Kalman Filter and Its Application to GPS Kinematic Positioning [J]. Journal of Tongji University: Natural Science Edition, 2011, 39(1): 124-128.

[17]穆静, 蔡远利. 迭代容积卡尔曼滤波算法及其应用 [J]. 系统工程与电子技术, 2011, 33(7): 1454-1457.

MU Jing, CAI Yuan-li. Iterated Cubature Kalman Filter and Its Application [J]. Systems Engineering and Electronic, 2011, 33(7): 1454-1457.

[18]霍庚, 杜军波, 丁宏升, 等. 光线SINS/GPS组合导航系统分析 [J]. 吉林大学学报: 信息科学版, 2012, 30(2): 144-150.

HUO Geng, DU Jun-bo, DING Hong-sheng, et al. Analysis and Study of Optical SINS/GPS Integrated Navigation System [J]. Journal of Jilin University: Information Science Edition, 2012, 30(2): 144-150.