第１３卷　第６期

２０１３年１２月

交 通 运 输 工 程 学 报

ＪｏｕｒｎａｌｏｆＴｒａｆｆｉｃａｎｄＴｒａｎｓｐｏｒｔａｔｉｏｎＥｎｇｉｎｅｅｒｉｎｇ

Ｖｏｌ．１３　Ｎｏ．６

Ｄｅｃ．２０１３

收稿日期：２０１３０７１８

基金项目：国家自然科学基金项目（６０９７９００２）；中央高校基本科研业务费专项资金项目（ＺＸＨ２０１２Ｄ００１）；中国民航大学科研基金项目

（２０１２ＫＹＥ０３）

作者简介：韩　萍（１９６６），女，天津人，中国民航大学教授，工学博士，从事数字信号处理与模式识别研究。

文章编号：１６７１１６３７（２０１３）０６０１１３０６

基于容积卡尔曼滤波的飞机姿态估计方法

韩　萍１，干浩亮２，何炜琨１，ＡＬＡＺＡＲＤＤａｎｉｅｌ３

（１．中国民航大学 天津智能信号与图像处理重点实验室，天津　３００３００；２．民航重庆空中交通管理分局，

重庆　４０１１２０；３．法国国立航空航天大学 数学与控制系，上加龙 图卢兹　３１５５５）

摘　要：为了提高非线性模型下飞机姿态估计的精度，建立了基于四元数与低精度高噪声传感器的

飞机姿态估计模型，应用基于球面径向积分准则的容积卡尔曼滤波算法进行姿态估计，通过实测数

据进行模型与算法验证，并与扩展卡尔曼滤波算法和中心差分卡尔曼滤波算法估计结果进行了比

较。对比结果表明：采用容积卡尔曼滤波算法能够有效提高飞机姿态估计的精度和稳定性，估计误

差最小，估计时间最短，而且，在运算过程中无需求导与可调参数。

关键词：姿态估计；捷联惯性导航系统；容积卡尔曼滤波；扩展卡尔曼滤波；中心差分卡尔曼滤波；四

元数

中图分类号：Ｖ２４９．３２　　　文献标志码：Ａ

犃犻狉犮狉犪犳狋犪狋狋犻狋狌犱犲犲狊狋犻犿犪狋犻狅狀犿犲狋犺狅犱犫犪狊犲犱狅狀犆犓犉

ＨＡＮＰｉｎｇ１，ＧＡＮＨａｏｌｉａｎｇ

２，ＨＥＷｅｉｋｕｎ１，ＡＬＡＺＡＲＤＤａｎｉｅｌ３

（１．ＴｉａｎｊｉｎＫｅｙＬａｂｏｒａｔｏｒｙｆｏｒＡｄｖａｎｃｅｄＳｉｇｎａｌＰｒｏｃｅｓｓｉｎｇ，ＣｉｖｉｌＡｖｉａｔｉｏｎＵｎｉｖｅｒｓｉｔｙｏｆＣｈｉｎａ，Ｔｉａｎｊｉｎ

３００３００，Ｃｈｉｎａ；２．ＣｈｏｎｇｑｉｎｇＡｉｒＴｒａｆｆｉｃＭａｎｇｅｍｅｎｔＳｕｂＢｕｒｅａｕｏｆＣｉｖｉｌＡｖｉａｔｉｏｎＡｄｍｉｎｉｓｔｒａｔｉｏｎｏｆ

Ｃｈｉｎａ，Ｃｈｏｎｇｑｉｎｇ４０１１２０，Ｃｈｉｎａ；３．ＤｅｐａｒｔｍｅｎｔｏｆＭａｔｈｅｍａｔｉｃｓａｎｄＣｏｎｔｒｏｌ，ＩｎｓｔｉｔｕｔＳｕｐéｒｉｅｕｒ

ｄｅｌＡéＲｏｎａｕｔｉｑｕｅｅｔｄｅＬＥｓｐａｃｅ，Ｔｏｕｌｏｕｓｅ３１５５５，ＨａｕｔｅＧａｒｏｎｎｅ，Ｆｒａｎｃｅ）

犃犫狊狋狉犪犮狋：Ｉｎｏｒｄｅｒｔｏｉｍｐｒｏｖｅｔｈｅｅｓｔｉｍａｔｉｏｎａｃｃｕｒａｃｙｏｆａｉｒｃｒａｆｔａｔｔｉｔｕｄｅｉｎｎｏｎｌｉｎｅａｒｍｏｄｅｌ，ａ

ｎｏｎｌｉｎｅａｒｅｓｔｉｍａｔｉｏｎｍｏｄｅｌｂａｓｅｄｏｎｔｈｅｑｕａｔｅｒｎｉｏｎａｎｄｔｈｅｓｅｎｓｏｒｓｗｉｔｈｌｅｓｓａｃｃｕｒａｃｙａｎｄｈｉｇｈｅｒ

ｎｏｉｓｅｗａｓｅｓｔａｂｌｉｓｈｅｄ，ａｎｄｔｈｅｃｕｂａｔｕｒｅＫａｌｍａｎｆｉｌｔｅｒ（ＣＫＦ）ａｌｇｏｒｉｔｈｍｂａｓｅｄｏｎｓｐｈｅｒｉｃａｌｒａｄｉａｌ

ｃｕｂａｔｕｒｅｒｕｌｅｓｗａｓａｐｐｌｉｅｄｔｏｅｓｔｉｍａｔｅｔｈｅａｔｔｉｔｕｄｅ．Ｔｈｅｍｏｄｅｌａｎｄａｌｇｏｒｉｔｈｍｗｅｒｅｖｅｒｉｆｉｅｄｂｙ

ｕｓｉｎｇｔｈｅｒｅａｌｆｌｙｉｎｇｄａｔａ．ＴｈｅｅｓｔｉｍａｔｉｏｎｒｅｓｕｌｔｓｏｆｃｕｂａｔｕｒｅＫａｌｍａｎｆｉｌｔｅｒａｌｇｏｒｉｔｈｍ，ｅｘｔｅｎｄｅｄ

Ｋａｌｍａｎｆｉｌｔｅｒ（ＥＫＦ）ａｌｇｏｒｉｔｈｍａｎｄｃｅｎｔｒａｌｄｉｆｆｅｒｅｎｃｅＫａｌｍａｎｆｉｌｔｅｒ（ＣＤＫＦ）ａｌｇｏｒｉｔｈｍ ｗｅｒｅ

ｃｏｍｐａｒｅｄ．ＣｏｍｐａｒｉｓｏｎｒｅｓｕｌｔｉｎｄｉｃａｔｅｓｔｈａｔｔｈｅｅｓｔｉｍａｔｉｏｎｐｒｅｃｉｓｉｏｎａｎｄｓｔａｂｉｌｉｔｙｏｆＣＫＦａｒｅ

ｈｉｇｈｅｒ，ｉｔｓｅｓｔｉｍａｔｉｏｎｔｉｍｅａｎｄｅｒｒｏｒａｒｅｍｉｎｉｍｕｍ，ａｎｄｔｈｅｄｅｒｉｖａｔｉｏｎａｎｄａｄｊｕｓｔａｂｌｅｐａｒａｍｅｔｅｒｓ

ａｒｅｎｏｔｒｅｑｕｉｒｅｄｉｎｔｈｅｅｓｔｉｍａｔｉｏｎ．１ｔａｂ，７ｆｉｇｓ，１５ｒｅｆｓ．

犓犲狔狑狅狉犱狊：ａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎ；ｓｔｒａｐｄｏｗｎｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｓｙｓｔｅｍ；ＣＫＦ；ＥＫＦ；ＣＤＫＦ；

ｑｕａｔｅｒｎｉｏｎ

犃狌狋犺狅狉狉犲狊狌犿犲：ＨＡＮＰｉｎｇ（１９６６），ｆｅｍａｌｅ，ｐｒｏｆｅｓｓｏｒ，ＰｈＤ，＋８６２０２４０９２４５０，ｈａｎｐｉｎｇｃａｕｃ＠

１６３．ｃｏｍ．

交　通　运　输　工　程　学　报 ２０１３年

０　引　言

飞机姿态是飞机导航控制中的重要参数［１］，通

常用一组欧拉角，即航向角、俯仰角和滚转角来表

示，它一方面为飞行员提供导航所需的飞机姿态信

息，另一方面也为自动驾驶仪、火力控制系统、雷达

天线及航空照相机云台等机载设备提供三维姿态基

准。精确测量飞机姿态信息，对于正确引导飞机飞

行，保证飞行安全具有重要作用。

飞机姿态角通常用捷联惯性导航系统来测量，

首先需建立描述飞机姿态角的数学模型，然后选用

合适的滤波估计算法去除噪声，得到最优姿态角。

由于描述飞机姿态角的数学模型存在严重的非线

性，对于非线性滤波问题，扩展卡尔曼滤波（Ｅｘｔｅｎｄ

ＫａｌｍａｎＦｉｌｔｅｒ，ＥＫＦ）算法应用最广泛［２］。ＥＫＦ算

法通过对非线性模型的泰勒级数展开式进行一阶线

性化截断，忽略高阶项，从而将非线性问题转换为线

性问题，这种近似处理方法会引入较大的线性化误

差，从而降低滤波精度，甚至导致发散。此外，ＥＫＦ

算法必须解算繁琐的雅克比矩阵，不适用于不连续

或不可导系统。针对ＥＫＦ算法的不足，Ｎｏｒｇａａｒｄ

等提出了基于Ｓｔｉｒｌｉｎｇ插值公式的中心差分卡尔曼

滤波（ＣｅｎｔｒａｌＤｉｆｆｅｒｅｎｃｅＫａｌｍａｎＦｉｌｔｅｒ，ＣＤＫＦ）算

法［３４］。ＣＤＫＦ算法根据Ｓｔｉｒｌｉｎｇ插值公式构造出

一组带权采样点，将采样点通过非线性函数来估计

状态的均值和协方差，具有比ＥＫＦ算法更小的线性

化误差和更高的滤波精度［５６］，但是，ＣＤＫＦ算法有

一个可调参数，在实际应用中需要合理调节，因此操

作不便，且需要２狀＋１个采样点，狀为系统状态变量

的维数。

加拿大学者Ａｒａｓａｒａｔｎａｍ等提出了基于球面径

向积分准则的容积卡尔曼滤波（ＣｕｂａｔｕｒｅＫａｌｍａｎ

Ｆｉｌｔｅｒ，ＣＫＦ）算法［７］，构造了２狀个具有相同权重的

容积点，通过非线性系统方程传播这些点进行状态

估计，无需对非线性模型线性化，其精度能达二

阶［８９］。ＣＫＦ算法比ＥＫＦ、ＣＤＫＦ等传统非线性滤

波算法具有更高的数值计算精度和滤波稳定性。

Ｌｉｕ等将ＣＫＦ算法应用于列车组合定位，取得了较

高的定位精度［１０］；Ｐｅｓｏｎｅｎ等运用ＣＫＦ算法研究了

导航定位问题，取得了良好的估计效果［１１］；Ｌｉ等将

ＣＫＦ算法应用于移动台的定位问题，取得了良好的

定位效果［１２］；鹿传国等将ＣＫＦ算法应用于纯方位

目标跟踪领域，取得了良好的跟踪效果［１３］。目前，

在飞机姿态估计领域，ＣＫＦ算法尚未见应用，因此，

本文将ＣＫＦ算法应用于基于四元数飞机姿态模型

的求解，利用从实际飞机模型上获取的飞行数据对

本文方法进行试验，并将试验结果与ＥＫＦ和ＣＤＫＦ

算法进行了比较。

１　容积卡尔曼滤波算法

容积卡尔曼滤波算法为

狓犽 ＝犳（狓犽－１）＋狏犽－１

狔犽 ＝犺（狓犽）＋狀烅烄

烆 犽

（１）

式中：犳（狓犽－１）、犺（狓犽）分别为狓犽－１、狓犽 的非线性状态

函数和非线性测量函数；狓犽、狔犽 分别为犽时刻的状

态变量和观测变量；狏犽－１为犽－１时刻的过程噪声；

狀犽 为犽时刻的观测噪声。

容积卡尔曼滤波算法的计算流程包括时间更新

和量测更新２部分［７］。

１．１　时间更新

时间更新过程为

犘犽－１ ＝犛犽－１犛Ｔ犽－１ （２）

犡犻，犽－１ ＝犛犽－１ξ犻＋狓!

犽－１（３）

犡犻，犽／犽－１ ＝犳（犡犻，犽－１） （４）

狓!

犽／犽－１ ＝１

犿∑犿

犻＝１

犡犻，犽／犽－１ （５）

犘犽／犽－１ ＝１

犿∑犿

犻＝１

犡犻，犽／犽－１（犡

犻，犽／犽－１）

Ｔ－

　　　 狓!

犽／犽－１狓!Ｔ

犽／犽－１＋犙犽－１ （６）

犿＝２狀

式中：犘犽－１为犽－１时刻的滤波的估计方差矩阵；犛犽－１

为犘犽－１的平方根矩阵；犡犻，犽－１为犽－１时刻的第犻个

Ｃｕｂａｔｕｒｅ点的更新状态向量；ξ犻为第犻个Ｃｕｂａｔｕｒｅ

点向量；狓!

犽－１为犽－１时刻的状态变量后验估计；

犡犻，犽／犽－１为犡犻，犽－１经过状态函数的更新向量；狓

!

犽／犽－１为

犽－１时刻的单步状态预测向量；犘犽／犽－１为犽－１时

刻的单步预测方差矩阵；犙犽－１为狏犽－１的协方差

矩阵。

１．２　量测更新

量测更新过程为

犘犽／犽－１ ＝犛犽／犽－１犛Ｔ犽／犽－１ （７）

犡犻，犽／犽－１ ＝犛犽／犽－１ξ犻＋狓!

犽／犽－１（８）

犢犻，犽／犽－１ ＝犺（犡犻，犽／犽－１） （９）

狔!

犽／犽－１ ＝１

犿∑犿

犻＝１

犢犻，犽／犽－１ （１０）

４１１

第６期 韩　萍，等：基于容积卡尔曼滤波的飞机姿态估计方法

犘狔犽 ＝１

犿∑犿

犻＝１

犢犻，犽／犽－１犢Ｔ犻，犽／犽－１－

　　 狔!

犽／犽－１狔!Ｔ

犽／犽－１＋犚犽 （１１）

犘狓犽狔犽 ＝１

犿∑犿

犻＝１

犡犻，犽／犽－１犢Ｔ犻，犽／犽－１－

　　 狓!

犽／犽－１狔!Ｔ犽／犽－１

（１２）

犓犽 ＝犘狓犽狔犽犘－１狔犽

（１３）

狓!

犽 ＝狓!

犽／犽－１＋犓犽（狔犽－狔!

犽／犽－１） （１４）

犘犽 ＝犘犽／犽－１－犓犽犘狔犽犓Ｔ犽 （１５）

式中：犛犽／犽－１为犘犽／犽－１的平方根矩阵；犡犻，犽／犽－１为犽－１

时刻的第犻个Ｃｕｂａｔｕｒｅ点的单步预测向量；犢犻，犽／犽－１

为犡犻，犽／犽－１经测量函数变换后对测量值的单步预测

向量；狔!

犽／犽－１为犽－１时刻量测的预测向量；犘狔犽为

犽－１时刻量测预测方差矩阵；犚犽 为狀犽 的协方差矩

阵；犘狓犽狔犽为犽－１时刻状态变量与量测变量的互协方

差矩阵；犓犽 为犽时刻的滤波增益。

图１　试验飞机模型

Ｆｉｇ．１　Ｅｘｐｅｒｉｍｅｎｔａｌａｉｒｃｒａｆｔｍｏｄｅｌ

２　飞机姿态估计系统建模

飞机模型上装有美国ＣＨＲｏｂｏｔｉｃｓ公司生产的

ＣＨＲ６ｄｍＡＨＲＳ传感器和法国ＳＢＧＳｙｓｔｅｍｓ公司

生产的ＳＢＧＡＨＲＳ传感器，见图１，均含有正交的

三轴陀螺仪、三轴加速度计和三轴磁强计，以捷联方

式工作。三轴陀螺仪测量飞机３个轴向的转动角速

率；三轴加速度计测量重力加速度在机体坐标系下

３个轴向的分量；三轴磁强计测量磁场矢量在机体

坐标系下３个轴向的分量。利用三轴加速度计和三

轴磁强计对地球重力场和地磁场的测量值补偿陀螺

仪的漂移，可以提高系统的精度，增强系统的稳定

性。ＣＨＲ６ｄｍＡＨＲＳ传感器和ＳＢＧＡＨＲＳ传感

器内部集成的ＥＫＦ算法可以实时提供飞机的航向

角、俯仰角、滚转角和未经处理的机体坐标系下的角

速率、重力矢量和地磁场矢量。ＳＢＧＡＨＲＳ传感器

的精度比ＣＨＲ６ｄｍＡＨＲＳ传感器的要高，但价格

也较高。

２．１　姿态估计方程

四元数法是飞机姿态描述的一种常用方法［１４］，

它不仅全局非奇异，而且运动学方程是双线性的。

定义姿态四元数犪、犫、犮、犱的向量狌为

　　　　　狌＝（犪，ρＴ）Ｔ

ρ＝（犫，犮，犱）Ｔ

且有 狌狌Ｔ＝‖狌‖２＝ρ

Ｔ

ρ＋犪２＝１

由刚体转动理论，得姿态四元数运动模型［１］为

ω＝ （狆，狇，狉）Ｔ

Ω＝

０ －狆 －狇 －狉

狆 ０ 狉 －狇

狇 －狉 ０ 狆

狉 狇 －狆

熿

燀

燄

燅０

狌·＝１

２Ω

烅

烄

烆狌

（１６）

式中：ω 为陀螺仪的角速度；Ω 为ω 的反对称矩

阵；狆、狇、狉分别为机体坐标系下绕狓、狔、狕轴转动的

真实角速率。

求解式（１６）可得到四元数，然后利用四元数与

姿态角之间的转换关系即可求得航向角φ、俯仰角θ

和滚转角ψ分别为

φ＝ａｒｃｔａｎ２（犪犫＋犮犱）

１－２（犫２＋犮

２［ ］）θ＝ａｒｃｓｉｎ［２（犪犮－犫犱）］

ψ＝ａｒｃｔａｎ２（犪犱＋犫犮）

１－２（犮２＋犱

２［ ］烅

烄

烆 ）

（１７）

２．２　姿态估计系统状态方程

本文用四元数来描述姿态，同时姿态信息需要

角速度，此外为了提高姿态估计的精度，用陀螺仪的

随机游走在线补偿陀螺仪的测量误差，故状态变量

包括四元数、角速度和陀螺仪的随机游走，即系统状

态变量为

狓＝（狌Ｔ，ω

Ｔ，犲Ｔ）Ｔ ＝

（犪，犫，犮，犱，狆，狇，狉，犲狆，犲狇，犲狉）Ｔ （１８）

式中：犲为陀螺仪角速率的随机游走；犲狆、犲狇、犲狉 分别

为陀螺仪的角速率狆、狇、狉的随机游走。

ＭＥＭＳ陀螺仪广泛采用的模型为［１５］

ω１ ＝ （狆１，狇１，狉１）Ｔ

犲＝ （犲狆，犲狇，犲狉）Ｔ

ω＝ω１－

烅

烄

烆 犲

（１９）

式中：ω１ 为陀螺仪的实际输出角速度；狆１、狇１、狉１ 为

５１１

交　通　运　输　工　程　学　报 ２０１３年

机体坐标系下陀螺仪实际输出的角速率。

最后可得到系统的非线性状态为

狆＝狆１－犲狆

狇＝狇１－犲狇

狉＝狉１－犲狉

狌·＝１

２Ω

烅

烄

烆狌

（２０）

２．３　姿态估计系统的观测方程

将三轴陀螺仪、三轴加速度计和三轴磁强计获

得的飞机机体坐标系下的转动角速率、重力矢量和

磁场矢量作为观测量，得到系统的观测变量为

狔＝（犃狓，犃狔，犃狕，犅狓，犅狔，犅狕，狆１，狇１，狉１）Ｔ （２１）

式中：犃狓、犃狔、犃狕 为机体坐标系下的重力观测值；

犅狓、犅狔、犅狕 为机体坐标系下的磁场观测值。

根据捷联惯性导航的基本原理，可以通过姿态

矩阵将导航坐标系的量转换成机体坐标系下的量，

观测变量方程可以表示为

狔＝

犠

犠

犐

熿

燀

燄

燅３×３

·

　　（犌犡，犌犢，犌犣，犕犡，犕犢，犕犣，狆１，狇１，狉１）Ｔ＋η （２２）

犠＝

犪２＋犫２－犮２－犱２ ２犫犮＋２犪犱 ２犫犱－２犪犮

２犫犮－２犪犱 犪２－犫２＋犮２－犱２ ２犮犱＋２犪犫

２犫犱＋２犪犮 ２犮犱－２犪犫 犪２－犫２－犮２＋犱

熿

燀

燄

燅２

式中：犌犡、犌犢、犌犣 分别为导航坐标系下３个轴向犡、

犢、犣上的重力，均为已知常量；犕犡、犕犢、犕犣 为导航

坐标系下的磁场值，均为已知常量；犐３×３为３阶单位

矩阵；η为零均值的高斯白噪声。

３　试验结果分析

试验中采样频率为１００Ｈｚ，状态变量的初值为

（０，０，０，１，０，０，０，０，０，０）Ｔ，误差协方差矩阵的初值

为１０４犐１０×１０，犐１０×１０为１０阶单位矩阵，中心差分区间

长度设为槡３。试验的初始阶段飞机是处于地面启

动状态，利用此时段采集的数据来计算观测噪声协

方差矩阵犚犽 和过程噪声协方差矩阵犙犽。此外，为

了验证在高噪声、低精度数据源情况下，ＣＫＦ算法估

计性能的有效性，以高精度ＳＢＧＡＨＲＳ传感器输出

的姿态估计结果作为标准，同时与ＥＫＦ算法和ＣＤＫＦ

算法估计的结果进行对比，试验结果见图２～４。为

了便于观察ＣＫＦ、ＣＤＫＦ和ＥＫＦ３种算法的估计效

果，将图２～４中的姿态角进行局部放大处理，见

图５～７。

图２～７中，ＣＤＫＦＣＨＲ、ＣＫＦＣＨＲ、ＥＫＦＣＨＲ

图２　航向角和航向角估计误差

Ｆｉｇ．２　Ｃｏｕｒｓｅａｎｇｌｅｓａｎｄｔｈｅｉｒｅｓｔｉｍａｔｉｏｎｅｒｒｏｒｓ

图３　俯仰角和俯仰角估计误差

Ｆｉｇ．３　Ｅｌｅｖａｔｉｏｎａｎｇｌｅｓａｎｄｔｈｅｉｒｅｓｔｉｍａｔｉｏｎｅｒｒｏｒｓ

分别表示用ＣＤＫＦ、ＣＫＦ、ＥＫＦ３种算法处理ＣＨＲ

６ｄｍＡＨＲＳ传感器采集的数据所得到的姿态角或

姿态角误差，ＥＫＦＳＢＧ表示用ＥＫＦ算法处理ＳＢＧ

ＡＨＲＳ传感器采集数据所得到的姿态角。可以看

出，在数据精度较低与噪声较大情况下，ＥＫＦ算法

的滤波性能最差，ＣＤＫＦ 算法滤波性能虽优于

ＥＫＦ，但在对姿态估计精度要求较高的场合，仍然

６１１

第６期 韩　萍，等：基于容积卡尔曼滤波的飞机姿态估计方法

图４　滚转角和滚转角估计误差

Ｆｉｇ．４　Ｒｏｌｌａｎｇｌｅｓａｎｄｔｈｅｉｒｅｓｔｉｍａｔｉｏｎｅｒｒｏｒｓ

图５　航向角局部放大曲线

Ｆｉｇ．５　Ｐａｒｔｉａｌｅｎｌａｒｇｍｅｎｔｃｕｒｖｅｓｏｆｃｏｕｒｓｅａｎｇｌｅｓ

图６　俯仰角局部放大曲线

Ｆｉｇ．６　Ｐａｒｔｉａｌｅｎｌａｒｇｍｅｎｔｃｕｒｖｅｓｏｆｅｌｅｖａｔｉｏｎａｎｇｌｅｓ

难以满足实际需要，而ＣＫＦ算法能够有效提高姿态

估计的精度。得到该结论的原因为ＥＫＦ算法通过

对非线性方程的泰勒级数展开式进行一阶截断，忽

略了高阶项，在计算状态变量后验分布中引入了较

大的线性化误差，因此，降低了滤波精度，而基于球

面径向积分原理的ＣＫＦ算法无需对模型线性化，对

状态变量的均值估计能达到２阶精度，因此，其姿态

图７　滚转角局部放大曲线

Ｆｉｇ．７　Ｐａｒｔｉａｌｅｎｌａｒｇｍｅｎｔｃｕｒｖｅｓｏｆｒｏｌｌａｎｇｌｅｓ

估计的精度高于ＥＫＦ算法，ＣＤＫＦ算法虽然对状态

估计的精度也能达到２阶，但是它有一个可调参数，

需要合理调节，才能获得较高的精度，而ＣＫＦ无可

调参数。

通过 ＭＡＴＬＡＢ平台对这３种算法的运算时间

进行了比较，结果见表１，可以看出，ＣＫＦ算法的运

算时间比ＣＤＫＦ缩短１７％，其原因为ＣＤＫＦ使用

２狀＋１个采样点，而ＣＫＦ只需２狀个采样点。

综上所述，从姿态估计精度和算法实现复杂度

上讲，相对于ＥＫＦ算法和ＣＤＫＦ算法，在飞机姿态

估计中ＣＫＦ算法是一种更好的滤波方法。

表１　三种算法运算时间比较

犜犪犫．１　犆狅犿狆狌狋犻狀犵狋犻犿犲犮狅犿狆犪狉犻狊狅狀狅犳３犪犾犵狅狉犻狋犺犿狊

算法 运算时间／ｓ

ＣＤＫＦ ４３．２０

ＣＫＦ ３５．８５

ＥＫＦ １７．９４

４　结　语

本文将容积卡尔曼滤波算法应用于由低精度高

噪声传感器组成的低成本飞机姿态估计系统中，并

利用实测数据进行验证。试验结果表明，ＣＫＦ算法

能有效提高飞机姿态估计的精度和稳定性，性能优

于ＥＫＦ和ＣＤＫＦ算法，同时在运算过程中无需求

导，无可调参数，实现容易，运算量适中，为飞机姿态

估计提供了一种新思路。如何对ＣＫＦ算法进行改

进以进一步提高其运算速度是下一步研究的重点。

参考文献：

犚犲犳犲狉犲狀犮犲狊：

［１］　ＣＲＡＳＳＩＤＩＳＪＬ，ＭＡＲＫＬＥＹＦＬ．Ｕｎｓｃｅｎｔｅｄｆｉｌｔｅｒｉｎｇｆｏｒ

ｓｐａｃｅｃｒａｆｔａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎ［Ｊ］．ＪｏｕｒｎａｌｏｆＧｕｉｄａｎｃｅ，Ｃｏｎｔｒｏｌ

ａｎｄＤｙｎａｍｉｃｓ，２００３，２６（４）：５３６５４２．

［２］　ＭＡＲＩＮＳＪＬ，ＹＵＮＸｉａｏｐｉｎｇ，ＢＡＣＨＭＡＮＮＥＲ，ｅｔａｌ．

ＡｎｅｘｔｅｎｄｅｄＫａｌｍａｎｆｉｌｔｅｒｆｏｒｑｕａｔｅｒｎｉｏｎｂａｓｅｄｏｒｉｅｎｔａｔｉｏｎ

７１１

交　通　运　输　工　程　学　报 ２０１３年

ｅｓｔｉｍａｔｉｏｎｕｓｉｎｇＭＡＲＧｓｅｎｓｏｒｓ［Ｃ］∥ＩＥＥＥ．Ｐｒｏｃｅｅｄｉｎｇｓｏｆ

ｔｈｅ２００１ＩＥＥＥ／ＲＳＪＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＩｎｔｅｌｌｉｇｅｎｃｅ

ＲｏｂｏｔｓａｎｄＳｙｓｔｅｍｓ．Ｍａｕｉ：ＩＥＥＥ，２００１：２００３２０１１．

［３］　ＮＯＲＧＡＡＲＤＭ，ＰＯＵＬＳＥＮＮＫ，ＲＡＶＮＯ．Ｎｅｗｄｅｖｅｌｏｐｍｅｎｔｓ

ｉｎｓｔａｔｅｅｓｔｉｍａｔｉｏｎｆｏｒｎｏｎｌｉｎｅａｒｓｙｓｔｅｍｓ［Ｊ］．Ａｕｔｏｍａｔｉｃａ，

２０００，３６（１１）：１６２７１６３８．

［４］　ＮＯＲＧＡＡＲＤＭ，ＰＯＵＬＳＥＮＮＫ，ＲＡＶＮＯ．Ａｄｖａｎｃｅｓｉｎ

ｄｅｒｉｖａｔｉｖｅｆｒｅｅｓｔａｔｅｅｓｔｉｍａｔｉｏｎｆｏｒｎｏｎｌｉｎｅａｒｓｙｓｔｅｍ［Ｒ］．

Ｃｏｐｅｎｈａｇｅｎ：ＴｅｃｈｎｉｃａｌＵｎｉｖｅｒｓｉｔｙｏｆＤｅｎｍａｒｋ，２００４．

［５］　ＭＥＲＷＥＲ，ＷＡＮＥ．ＳｉｇｍａｐｏｉｎｔＫａｌｍａｎｆｉｌｔｅｒｓｆｏｒｐｒｏｂａｂｉｌｉｓｔｉｃ

ｉｎｆｅｒｅｎｃｅｉｎｄｙｎａｍｉｃｓｔａｔｅｓｐａｃｅｍｏｄｅｌｓ［Ｄ］．Ｐｏｒｔｌａｎｄ：Ｏｒｅｇｏｎ

ＨｅａｌｔｈａｎｄＳｃｉｅｎｃｅＵｎｉｖｅｒｓｉｔｙ，２００４．

［６］　ＷＵＹｕａｎｘｉｎ，ＨＵＤｅｗｅｎ，ＷＵＭｅｉｐｉｎｇ，ｅｔａｌ．Ａｎｎｕｍｅｒｉｃａｌ

ｉｎｔｅｇｒａｔｉｏｎｐｅｒｓｐｅｃｔｉｖｅｏｎＧｕａｓｓｉａｎｆｉｌｔｅｒｓ［Ｊ］．ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓ

ｏｎＳｉｇｎａｌＰｒｏｃｅｓｓｉｎｇ，２００６，５４（８）：２９１０２９２１．

［７］　ＡＲＡＳＡＲＡＴＮＡＭＩ，ＨＡＹＫＩＮＳ．ＣｕｂａｔｕｒｅＫａｌｍａｎｆｉｌｔｅｒｓ［Ｊ］．

ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓｏｎ ＡｕｔｏｍａｔｉｃＣｏｎｔｒｏｌ，２００９，５４（６）：

１２５４１２６９．

［８］　ＡＲＡＳＡＲＡＴＮＡＭＩ．ＣｕｂａｔｕｒｅＫａｌｍａｎｆｉｌｔｅｒｉｎｇ：ｔｈｅｏｒｙａｎｄ

ａｐｐｌｉｃａｔｉｏｎｓ［Ｄ］．Ｈａｍｉｌｔｏｎ：ＭｃＭａｓｔｅｒＵｎｉｖｅｒｓｉｔｙ，２００９．

［９］　ＦＥＭＡＮＤＥＺＰＲＡＤＥＳＣ，ＶＩＬＡＶＡＬＬＳＪ．Ｂａｙｅｓｉａｎｎｏｎｌｉｎｅａｒ

ｆｉｌｔｅｒｉｎｇｕｓｉｎｇｑｕａｄｒａｔｕｒｅａｎｄｃｕｂａｔｕｒｅｒｕｌｅｓａｐｐｌｉｅｄｔｏｓｅｎｓｏｒ

ｄａｔａｆｕｓｉｏｎｆｏｒｐｏｓｉｔｉｏｎｉｎｇ［Ｃ］∥ＩＥＥＥ．２０１０ＩＥＥＥＩｎｔｅｒｎａｔｉｏｎａｌ

ＣｏｎｆｅｒｅｎｃｅｏｎＣｏｍｍｕｎｉｃａｔｉｏｎｓ．ＣａｐｅＴｏｗｎ：ＩＥＥＥ，２０１０：１５．

［１０］　ＬＩＵＪｉａｎｇ，ＣＡＩＢａｉｇｅｎ，ＴＡＮＧＴａｏ，ｅｔａｌ．ＡＣＫＦｂａｓｅｄ

ＧＮＳＳ／ＩＮＳｔｒａｉｎｉｎｔｅｇｒａｔｅｄｐｏｓｉｔｉｏｎｉｎｇｍｅｔｈｏｄ［Ｃ］∥ＩＥＥＥ．

２０１０ＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＭｅｃｈａｔｒｏｎｉｃｓａｎｄＡｕｔｏｍａｔｉｏｎ．

Ｘｉａｎ：ＩＥＥＥ，２０１０：１６８６１６８９．

［１１］　ＰＥＳＯＮＥＮＨ，ＰＩＣＨＥＲ．ＣｕｂａｔｕｒｅｂａｓｅｄＫａｌｍａｎｆｉｌｔｅｒｓｆｏｒ

ｐｏｓｉｔｉｏｎｉｎｇ［Ｃ］∥ＩＥＥＥ．Ｐｒｏｃｅｅｄｉｎｇｓｏｆｔｈｅ７ｔｈＷｏｒｋｓｈｏｐｏｎ

Ｐｏｓｉｔｉｏｎｉｎｇ，ＮａｖｉｇａｔｉｏｎａｎｄＣｏｍｍｕｎｉｃａｔｉｏｎ．Ｄｒｅｓｄｅｎ：ＩＥＥＥ，

２０１０：４５４９．

［１２］　ＬＩＷｅｎｌｉｎｇ，ＪＩＡＹｉｎｇｍｉｎｇ．Ｌｏｃａｔｉｏｎｏｆｍｏｂｉｌｅｓｔａｔｉｏｎｗｉｔｈ

ｍａｎｅｕｖｅｒｓｕｓｉｎｇａｎＩＭＭｂａｓｅｄｃｕｂａｔｕｒｅＫａｌｍａｎｆｉｌｔｅｒ［Ｊ］．

ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓｏｎＩｎｄｕｓｔｒｉａｌＥｌｅｃｔｒｏｎｉｃｓ，２０１２，５９（１１）：

４３３８４３４８．

［１３］　鹿传国，冯新喜，张　迪．基于改进容积卡尔曼滤波的纯方位

目标跟踪［Ｊ］．系统工程与电子技术，２０１２，３４（１）：２８３３．

ＬＵＣｈｕａｎｇｕｏ，ＦＥＮＧ Ｘｉｎｘｉ，ＺＨＡＮＧ Ｄｉ．Ｐｕｒｅｂｅａｒｉｎｇ

ｔｒａｃｋｉｎｇ ｂａｓｅｄ ｏｎｉｍｐｒｏｖｅｄ ｃｕｂａｔｕｒｅ Ｋａｌｍａｎｆｉｌｔｅｒ［Ｊ］．

ＳｙｓｔｅｍｓＥｎｇｉｎｅｅｒｉｎｇａｎｄＥｌｅｃｔｒｏｎｉｃｓ，２０１２，３４（１）：２８３３．

（ｉｎＣｈｉｎｅｓｅ）

［１４］　ＳＨＵＳＴＥＲ Ｍ Ｄ．Ｓｕｒｖｅｙｏｆａｔｔｉｔｕｄｅｒｅｐｒｅｓｅｎｔａｔｉｏｎｓ［Ｊ］．

ＪｏｕｒｎａｌｏｆｔｈｅＡｓｔｒｏｎａｕｔｉｃａｌＳｃｉｅｎｃｅｓ，１９９３，４１（４）：４３９５１７．

［１５］　ＫＡＲＬＧＡＡＲＤＣ，ＳＣＨＡＵＢＨ．Ａｄａｐｔｉｖｅｈｕｂｅｒｂａｓｅｄｆｉｌｔｅｒｉｎｇ

ｕｓｉｎｇｐｒｏｊｅｃｔｉｏｎｓｔａｔｉｓｔｉｃｓ：ａｐｐｌｉｃａｔｉｏｎｔｏｓｐａｃｅｃｒａｆｔａｔｔｉｔｕｄｅ

ｅｓｔｉｍａｔｉｏｎ［Ｃ］∥ＡＩＡＡ．２００８ＡＩＡＡＧｕｉｄａｎｃｅ，Ｎａｖｉｇａｔｉｏｎ，ａｎｄ

ＣｏｎｔｒｏｌＣｏｎｆｅｒｅｎｃｅ．Ｈｏｎｏｌｕｌｕ：ＡＩＡＡ，２００８：

１２０．

（上接第１１２页）

　　　Ｑｕａｌｉｔｙ，２００９（５）：６８．（ｉｎＣｈｉｎｅｓｅ）

［７］　ＺＨＡＯ Ｌｉｎｈａｉ，ＬＩＺｈａｎｋｕｉ，ＬＩＵ Ｗｅｉｎｉｎｇ，ｅｔａｌ．Ａｎ

ｉｎｔｅｇｒａｔｅｄｕｐｌｉｎｋｓｉｇｎａｌｄｅｔｅｃｔｉｏｎ ｍｅｔｈｏｄｏｆｒａｉｌｗａｙｂａｌｉｓｅ

ｓｙｓｔｅｍｂａｓｅｄｏｎｗａｖｅｌｅｔｒｉｄｇｅ［Ｃ］∥ＩＥＥＥ．２００９３ｒｄＩＥＥＥ

ＩｎｔｅｒｎａｔｉｏｎａｌＳｙｍｐｏｓｉｕｍｏｎＭｉｃｒｏｗａｖｅ，Ａｎｔｅｎｎａ，Ｐｒｏｐａｇａｔｉｏｎ

ａｎｄ ＥＭＣ Ｔｅｃｈｎｏｌｏｇｉｅｓ ｆｏｒ Ｗｉｒｅｌｅｓｓ Ｃｏｍｍｕｎｉｃａｔｉｏｎｓ．

Ｂｅｉｊｉｎｇ：ＩＥＥＥ，２００９：７８８３．

［８］　徐　宁，张季良，王财进，等．列车高速运行条件下应答器车载

测试设备关键技术［Ｊ］．中国铁道科学，２０１０，３１（４）：１３１１３７．

ＸＵ Ｎｉｎｇ，ＺＨＡＮＧＪｉｌｉａｎｇ，ＷＡＮＧ Ｃａｉｊｉｎ，ｅｔａｌ．Ｔｈｅ

ｃｒｕｃｉａｌｔｅｃｈｎｉｑｕｅｓｏｆｂａｌｉｓｅｏｎｂｏａｒｄｔｅｓｔｅｑｕｉｐｍｅｎｔｆｏｒｈｉｇｈ

ｓｐｅｅｄｔｒａｉｎ［Ｊ］．ＣｈｉｎａＲａｉｌｗａｙＳｃｉｅｎｃｅ，２０１０，３１（４）：１３１

１３７．（ｉｎＣｈｉｎｅｓｅ）

［９］　曾洁毅，赵会兵．应答器传输模块测试系统的研究［Ｊ］．北京

交通大学学报，２００８，３２（２）：８０８３．

ＺＥＮＧＪｉｅｙｉ，ＺＨＡＯＨｕｉｂｉｎｇ．Ｒｅｓｅａｒｃｈｏｎｂａｌｉｓｅｔｒａｎｓｍｉｓｓｉｏｎ

ｍｏｄｕｌｅｔｅｓｔｓｙｓｔｅｍ［Ｊ］．ＪｏｕｒｎａｌｏｆＢｅｉｊｉｎｇＪｉａｏｔｏｎｇＵｎｉｖｅｒｓｉｔｙ，

２００８，３２（２）：８０８３．（ｉｎＣｈｉｎｅｓｅ）

［１０］　王　瑞，赵会兵，王舒民．面向应答器传输模块测试的上行链

路信号模拟器研究［Ｊ］．铁道学报，２００８，３０（６）：４６５０．

ＷＡＮＧＲｕｉ，ＺＨＡＯ Ｈｕｉｂｉｎｇ，ＷＡＮＧＳｈｕｍｉｎ．Ｒｅｓｅａｒｃｈ

ｏｎｕｐｌｉｎｋｓｉｇｎａｌｓｉｍｕｌａｔｏｒｕｓｅｄｆｏｒＢＴＭｔｅｓｔｉｎｂａｌｉｓｅｓｙｓｔｅｍ［Ｊ］．

ＪｏｕｒｎａｌｏｆｔｈｅＣｈｉｎａＲａｉｌｗａｙＳｏｃｉｅｔｙ，２００８，３０（６）：４６５０．

（ｉｎＣｈｉｎｅｓｅ）

［１１］　ＺＨＡＯＬｉｎｈａｉ，ＪＩＡＮＧＹｉｎｇ．Ｍｏｄｅｌｉｎｇａｎｄｓｉｍｕｌａｔｉｏｎｏｆｂａｌｉｓｅ

ｕｐｌｉｎｋｄａｔａｔｒａｎｓｍｉｓｓｉｏｎｂａｓｅｄｏｎｆｉｎｉｔｅｅｌｅｍｅｎｔｍｅｔｈｏｄ［Ｊ］．

ＪｏｕｒｎａｌｏｆＴｈｅｏｒｅｔｉｃａｌａｎｄＡｐｐｌｉｅｄＩｎｆｏｒｍａｔｉｏｎＴｅｃｈｎｏｌｏｇｙ，

２０１２，４６（２）：８６７８７４．

［１２］　胡爱群，苏　杰．自适应ＦＳＫ解调方法［Ｊ］．应用科学学报，１９９６，

１４（２）：１６７１７２．

ＨＵＡｉｑｕｎ，ＳＵＪｉｅ．Ａｎａｄａｐｔｉｖｅｄｅｍｏｄｕｌａｔｉｏｎｍｅｔｈｏｄｆｏｒ

ＦＳＫｓｉｇｎａｌｓ［Ｊ］．ＪｏｕｒｎａｌｏｆＡｐｐｌｉｅｄＳｃｉｅｎｃｅ，１９９６，１４（２）：

１６７１７２．（ｉｎＣｈｉｎｅｓｅ）

［１３］　ＺＨＡＯＺｈｉｊｉｎ，ＦＵＢａｉｃｈｅｎｇ，ＸＵＣｈｕｎｙｕｎ．Ａｎａｄａｐｔｉｖｅ

ｄｅｍｏｄｕｌａｔｉｏｎｍｅｔｈｏｄｆｏｒＭＦＳＫｓｉｇｎａｌｓｕｎｄｅｒａｌｐｈａｓｔａｂｌｅ

ｄｉｓｔｒｉｂｕｔｉｏｎｐｕｌｓｅｎｏｉｓｅ［Ｃ］∥ＩＥＥＥ．２００８ＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｇｒｅｓｓ

ｏｎＩｍａｇｅａｎｄＳｉｇｎａｌＰｒｏｃｅｓｓｉｎｇ．Ｓａｎｙａ：ＩＥＥＥ，２００８：６５６９．

［１４］　李炎新．调制信号的自适应解调方法研究［Ｄ］．南京：东南

大学，２００９．

ＬＩＹａｎｘｉｎ．Ｒｅｓｅａｒｃｈｏｎｔｈｅｍｅｔｈｏｄｏｆａｄａｐｔｉｖｅｌｙｄｅｍｏｄｕｌａｔｉｎｇ

ｔｈｅｍｏｄｕｌａｔｅｄｓｉｇｎａｌ［Ｄ］．Ｎａｎｊｉｎｇ：ＳｏｕｔｈｅａｓｔＵｎｉｖｅｒｓｉｔｙ，

２００９．（ｉｎＣｈｉｎｅｓｅ）

［１５］　钱志文．基于自适应滤波的ＡＳＫ解调方法及性能［Ｊ］．工矿自

动化，２００３（３）：１４１６．

ＱＩＡＮＺｈｉｗｅｎ．ＡＡＳＫｄｅｍｏｄｕｌａｔｉｏｎｍｅｔｈｏｄｂａｓｅｄｏｎｓｅｌｆ

ａｄａｐｔｉｖｅｆｉｌｔｅｒｉｎｇａｎｄｉｔｓｐｅｒｆｏｒｍａｎｃｅ［Ｊ］．ＩｎｄｕｓｔｒｙａｎｄＭｉｎｅ

Ａｕｔｏｍａｔｉｏｎ，２００３（３）：１４１６．（ｉｎＣｈｉｎｅｓｅ）

８１１