基于自适应UKF微型航姿系统噪声的在线估计

1、基于自适应ukf微型航姿系统噪声在线估计刘宇刘琼周帆李云梅向磊（垂庆邮电人学光电信息感测与传输技术巫庆市重点实验空，巫庆400065）摘 要：针对先验噪声与系统克实噪声不符引起标准无迹卡尔曼（ukf）性能退化的情况，提出了一种应用于非线性吋变状 态和参数联合估计的自适应ukf（aukf）算法。该算法首先基于新的协方差矩阵与相应估计值z间存在的误差，构建成本 函数。然后为了最小化成木函数，采用梯度下降法进行在线预估。最后对噪声的协方差进行在线更新，将更新后的协方差反 馈给标准的ukf,使更新后的理论协方差接近真实噪声。通过实验仿真分析，口适应ukf相比标准ukf,静态精度提高了 40%,动态精度

2、提高了 30%,验证了自适应ukf噪声在线估计模型的准确性和可行性。在实际测试系统中，对于时变噪声 协方差不确定吋，自适应ukf噪声在线估计的鲁棒性得到明显改善。关键词：ukf ； aukf；联合估计；成本函数；梯度下降算法；鲁棒性中图分类号：tn212文献标识码：anoise on-line estimation of mini-ahrs based on adaptive ukf algorithmyu liu, qiong liu,(chongqing municipal level key laboratory of pholoclcctronic information sensin

3、g & transmitting technology,chongqing university of posts and telecommunications. chongqing, 400065, china)abstract: considering that the prior noise of a normal unscented kalman filter does not agree with its real behavior, an adaptive unscented kalman filter algorithm applied to nonlinear join

4、t estimation of both time-varying states and parameters is proposed. firstly, a cost function is built based on the error between the covariance matrices of innovation and their corresponding estimations. then in order to minimize the cost function, the gradient descent method for on-line forecast i

5、s used. finally, the noise covariance is online updated, the updated covariance feedback to the standard ukf, which makes the updated theoretical covariance closer to the true noise. through experimental simulation analysis, compared adaptive ukf with standard ukf, static precision is improved by 40

6、%, dynamic precision is improved by 30%, the accuracy and feasibility of online adaptive ukf noise estimation model is verified. in the actual test system, for time-varying noise covariance is uncertain, adaptive ukf online noise estimation robustness are improved significantlykey words: ukf;aukf; j

7、oint cstimation;cost function; gradient dcsccnt;robustncss引言由于微型航姿系统的复杂性和时变性，为了让 该系统得到精确的姿态角新息，准确的估计状态和 参数是很关键的。但传统的估计方法具有明显的缺 陷，如仅适用于线性时不变系统、对目标模型的强 依赖性、对突变状态跟踪的滞后性等，这些都大大 限制英工程应用。所以对航姿系统的噪声进行实时 的估计。将成为提高微型姿态仪精度的关键技术。 近些年来，人们在在线建模领域取得了骄人的成 绩，参考模型控制领域正在成为一个重要的研究方 向。对于非线性系统来说，普遍运用的估计器就 是扩展卡尔曼滤波（ekf）。

8、尽管它被广泛应用，但 它在估计过程中仍然冇一些缺陷，比如动态状态的 可导性，对偏差和散度敏感。无迹r尔曼滤波器 （ukf）,使用非线性动态方程直接代替了线性方程。 ukf与ekf相比,具有相同的复杂度,都是o（l3） 阶。因为ukf使用的非线性方程并没有线性化， 因此不需要估计雅可比行列式或者hessians矩阵就 能实现二阶精度（ekf的精度是一阶）。然而ukf 也是卡尔曼滤波器的扩充，它和卡尔曼滤波一样， 它只有在知道一些假定信息作为先验信息的情况 下冲叫才能实现优异的性能。这些先验信息包括： 1）精确参考模型；2）噪声分布的完整信息；3）正确 的初始条件。但是，因为先验信息会被时变和不确

9、 定的环境所影响，所以不能保证很强的稳定性。克 服上述提缺陷的一种有效方式是使用自适应算法。 国内外在自适应滤波领域已有许多研究。huetal 提出了限制kf记忆的方法，这种方式能根据最优化条件来自适应调整遗忘因子。may beck罔设计一 个自适应kf,通过最大似然估计的方法估计错误 统计的协方差矩阵。loebis et al叫吏用了模糊逻辑 技术来更新传感器噪声方差。但是在实际屮，每次 要立即确定协方差的增量值是困难的。在本论文 中，提出了自适应无迹卡尔曼滤波，它通过新的协 方差矩阵和它相应的估计之间的误差建立了成本 函数，然后将更新好的协方差反馈到该系统，实现 系统噪声的在线估计。1 u

10、kf滤波算法11 ukf滤波的算法原理假设微型航姿系统的离散方程和观测方程分 别为：xhk、-1-1(i)zk=h(xk) + ek(2)对以知道，ukf算法中的观测方程也是非线性 函数。状态向量笛和观测向量乙所包含坐标向量 数量，根据实际的微型航姿系统三维姿态数据确 定。1.2标准ukf的计算步骤(1) 利用初始状态估计，设计最初的2n+l个 sig伽点：ukf会将噪声项添加到状态项屮，即状 态扩维，并将驱动噪声阵q扩到p阵中。进行状态扩 充的原因是考虑噪声对系统的影响，进行扩维以后 的状态变量和其他协方差矩阵：pk 0 0 p; = o a o_0 0 rk设賈为l维列向量，显然l二n+p

11、+q。(2) 利用过程模型变换这些sgam点，即对状态 变量进行u变换：xf(k + 1/ k) = fxt (kl k)yu(k)yk(5)式中/卜为u变换的具体方程，步骤如下：(3)(4)(6)=匹十(电亘埜)-.(心1,.,励(?)xk - (j(2 + yl)yx(,=加 +1,2m)(11)(12)(13)(14)昭伽+ 2)(8)= a /(m + 2) + (1 -+ /?)(9)叫”二必=0.5 皈+2 )j=( 皿 (10)式中加为状态参数个数,兄二小加+归-加为尺 度因子，其中的a表示sig伽点到乞的距离，一般 将这个数值设置为104<a<l, k为常数，一般设

12、 为0或者为3-7, “用于融入预报向量笛的验前信 息，对于高斯分布，0=2最优。(j(m+久)工元)是 矩阵平方根的第i列，可以通过cholesky分解获得。(3) 计算预测估计值f伙+ 1/灯=£比川2“ + 1/幻/h)(4) 计算预测协方差p(k+l/k) = £比叫 (k + l/k)-x 伙 + 1/幻丁/=0(5) 通过测量方程计算测量值乙伙 + /k) = hx 伙+ 1/幻(6) 计算预测测量值2nz(k + / k) = wi(c)zi(k + / k)-.r=0八z伙+1/幻乙伙+1/切一z伙+1/灯r+&式屮，假设预报残差向量为k =z

13、63;k + /k)-zk + /k)可以知道向量确定等价 权戸，进而求出。(7) 计算去仏+1/幻和2伙+ 1/灯的协方差化伙+ 1/約二左比尢伙+ 1/幻八1=0、x(k +1 / r)z“ +1 / 約 一 z 伙 +1 / 加(8) 计算卡尔曼增益k( £+ 1 )=匕 i(9) 更新误差协方差p伙+ 1/比+ 1)=卩伙+ 1/灯伙+ 1)冒7伙+ 1)(10) 更新状态x( ah- 1 /总 1#x *+1 £k(心1) £ 匚)z幷(15)(16)(17)(18)ukf其实就是状态估计方法的一种，同过联合 估计系统参数，将模型参数作为动态变量，将其添

14、加到真实的状态矢量，合并成增广状态矢量，利用 自适应ukf对状态和模型参数进行预测估计。ukf 方法其关键技术就是对sigma点采样策略，包括 sigma点个数的确定，位置的确定，和相应权值的确 定，目前大多数研究表明，采用2n+l个sigma点， 它们服从x分布，并且当前的状态值与方差和样本 均值相同。2自适应ukf滤波算法2.1成本指标函数本文主要是将最小化真实值和新息方差的差 作为成本指标函数，将滤波更新中产生的真实值和 估计状态值差值来更新滤波器的参数。yr （式中n是估计窗口的尺度因子，心是新息，vk 可以这样描述：u =儿-加t（2°）式子中，儿和”-是滤波和预测的真实值

15、。相 刈來说，我们可以通过标准ukf知道：$=：£町亿“匸孤门兔- w） /=0然后提出自适应的成本指标函数，以尽量减少 成本函数。k = "3（） = "（/ 一 £ 尸（22）式中，t是取样时间或者恒定的时间间隔。式 （25）需要结合常规的ukf來实现自适应ukf。rfl 式（22）可得：(26)以根据式（19）, （20）可以推导出:(27)根据式（21）,可以推导出：务=z吋一愛p（兀詢1 一）t販 r=0 两(28)为了计算两爲辿，必须先求先ukf的滤 波方程的导数。参照标准ukf算法，对应的可以 得到叽闷的迭代计算公式如下所述：2.2自适应u

16、kf实际上，方差矩阵q和r是决定精度和稳定性的 因素，因此，选择协方差的对角元素去自适应更新 过程噪声q。不失一般性,r也可以通过该方法更新。 whittake准则是一种比较常规的口适应参数更新算 法，方法简单，计算量少。此方法就是基于梯度下 降算法，将负梯度方向作为参数更新的方向。即：sigma点的导数wa-i(29)(23)式中，q：为时刻噪声方差阵第m行的对角线元 素；qk是控制收敛速度的自适应调节率，其符合传 统的随机佔计条件久o' »人=出（24）kk根据式（23）,我们可以得到下面递归机制：(25)p z =)pz = n + l,.2n2 j加 wkj/? +

17、兄(（1）初始化测量更新:),/ = 1,川dq； <72如.=血二l 一 厶 + 2.().,/ = « + 1,.2川dq；1 dx x=ximl 两"梯度预测:；=0'勿;”勿;"八汗如 +(z)(空吐至zur_， '阻民=_p:刃d p-< 砸砸&kk _ p-i _ p- p- °仏<砸祸饰dq；opk _ d&w _ 6kk p _ 尺績齐 y 議一市一議砸一八市 函 _ cxkk- gcyk- 一+議6皿)一瓦研)(33)3系统试验及分析3.1仿真平台在本论文中，提出的自适应ukf运用在自主

18、 研制的微型航姿系统(参见图1),微型航姿系统 主要是由三轴陀螺仪、三轴加速度和三轴磁强计组 成，根据惯性导航的相关理论，采用东北天导航坐 标描述载体姿态新息重要模型。图1门主研制的微型姿态仪3.2微型航姿系统的测量的系统结构本文提出的是基于自适应ukf噪声的在线估 计，系统利用加速度和磁力计计算出姿态，将陀螺 的输出反馈到动态解算过程中，利用自适应ukf 实现对磁力计和加速度的噪声估计，1司时也用磁力 计和加速度计対陀螺的累积误差进行在线预估。微 型航姿系统的数学模型如图2所示。图2微型航姿系统测戢的系统结构3.3微型姿态仪的状态模型在惯性坐标系屮，定义姿态角为(0 &, 0, 通过

19、姿态角的导数，对磁力计和加速度动态测量的 误差进行补偿，此时，在对自适应ukf的状态模 型进行设计时，采用陀螺仪的数据，但是在k+1时 刻姿态角通过该时刻角速度在釆样时间内的积分， 如式(34):(32)/ 7、= rr(x,(fi)s、+疋(兀,0)疋(幵0)"0)0(34)丿allju丿+ 卩(兀,0)疋(”&) 0sinta n&cos tan 0、/ 、%=0cos0sin0/<0sin 0/cos。cos 0 / cos 0 jm丿(35) 式(35),等式左边是姿态角，它是通过磁力计和 加速度进行解算的，把这个解算出来的姿态角作为 自适应ukf算法的

20、观测值，式(36)观测模型。丿 、 丿 oo o oo o o o o o o 1 o o 1 1 o o=、 丿(36由于时间的积累，根据ukf的状态方程，将 前一个时刻计算的结果作为下一个时刻角速度，对 其做一个简单的预估计，但是估计中会带來积累误 差，如式(37) o(37)本论文将磁力计和加速度讣解算后的姿态角 作为观测值，用估算值通过加权计算去求得z,+1, 然后经过式(38)去补偿陀螺长时间带来的积累误 差,m是卡尔曼的增益。兀；+1-母+i = m(z&+ -z+j(38)式(37)是陀螺仪的输出，通过状态转移可以描 述其中的动态过程，能够补偿通过磁力计和加速度 动态测量

21、的误差，通过反馈，能够克服长时间陀螺 的积累误差。4仿真验证为了验证自适应ukf算法的效果，实验对自 主研制的微型航姿系统进行数据釆集，将算法运用 到该系统中。采样时间选择30s,采样频率为50h乙 为了验证该算法的优越性，将自适应ukf算法和 普通的ukf算法进行对比，分別做了静态和动态 实验。由于横滚角和俯仰角在算法的原理上一样， 为避免赘述，所以本实验主要对航向和俯仰进行验 证。图3分别是静态和动态俯仰角和航向角偏差。 图中的蓝线为标准ukf算法获得的误差曲线，图 中的红线是自适应ukf算法获得的误差曲线，黑 色的线是期望得到的值。图中曲线表明，静态实验, 采用标准ukf航向角偏差在&#

22、177;0.5,静态俯仰角偏 差在土 1°,然而自适应ukf算法静态航向角偏差在 0.3°,俯仰角偏差在o.zo动态实验，标准ukf 航向角偏差在±2°,俯仰角偏差在0.75 o自适应 ukf航向角偏差在1°,俯仰角偏差在0.4实验验 证表明，自适应ukf算法在动静态实验中都比标 准的ukf算法对误差估计的精度要高。ukf不能 得到较好的估计性能；而自适应ukf算法可以很 快地克服噪声特性的改变。1r曲mm 自般urng谿割200 ioo 6938031»312001400m.（+）（a）静态俯仰角偏差叶礎ukf 自诙趙能：00.5-

23、0.57 (9*二eeg 掘辕203 ioc 6008301 00012001400(b) 静态航向角偏差酬ukf方法1.5一自适应ukf方法期璽能91.5200400600800100012001400耕点（个）(c) 动态俯仰角偏差3-2觥ukf施自駆mm 期鏗020340c6c010m1203 u0co!二匸【£苏祓w(d) 动态航向角偏差图3动静态姿态角偏差本实验是使用加速度计，磁强计和陀螺仪，静 态是直接把微型航姿系统放在双轴转台上。动态采 用的是摇摆实验，采用标准ukf和自适应ukf算 法对传感器数据融合后，静态动态仿真结果如图(4) 所示：图屮表明，自适应ukf较标准u

24、kf对噪声 的在线估计精度高，在静态的自适应程度较动态 高。对噪声的精确估计后，使微型航姿系统的静态 俯仰角精度在±0.2°,航向角精度在土0.5°o动态 俯仰角精度土0.8°,动态航向角精度±1°。已经满952co40cs30it)儆1200um(a)静态俯仰图r7170t羽宅*»600 a» 1occ 1203 14co采滋(个)26$(b)静态航向角1co册1i111 加11'1ico4cc 6m 800m (t)梯umi自和imf旋1»j120014(0(c)动态俯仰角loo - *1 卅

25、111卅 15edlki访法ij bgguff 施10001200 uof(d)动态航向角 图4动静态姿态角本论文针对传统ukf因为假定的先验噪声与 实际系统噪声不符导致精度下降，提出了一种基于 自适应无迹卡尔曼滤波器(aukf),构建以新的协方 差矩阵与相应估计值误差为成本函数，将更新的协 方差反馈到实测系统，对自主研制的微型航姿系统 的噪声进行在线估计，实验仿真表明，静态精度提 高了 40%,动态精度提高了 30%,验证了在线估计 模型的正确性和可行性。有很好的工程应用价值。 参考文献：1 徐慧娟，吴美平，罗兵.ekf和ukf ins/gps组合导航中的应用分析j.航天控制,2006,26(6):7-10.xu huijuan, wu mciping, luo bing.analysis of extended and unscented kalman filtering for ins/gps integrationfj.a erospace con trol,2006,26:710.2 张欣，口越，赵常均.多旋翼姿态解算中的改进自适应扩 展kalman算法j.光学精密工程,2014zhang xin, bai yuc, zhao changjun.improved adaptive extended kalman algorithm