算法分析
数字共本与成用
基于matlab的四元数卡尔曼滤波组合导航算法
于晓磊陈维民毕嘉城谢光辉
(哈尔滨理工大学黑龙江哈尔滨150000)
摘要：随着无人机技术的不断发展，为了更加全面的实现无人机在不同环境中的姿态解算问题，组合导航算法越来越受到重视成为日前无人机的研究重点之一,传统四旋翼控制采用融合滤波、互补滤波算法,在姿态解算上有着较好的效果,并且计算速度很快,但是,达种算法在实时性上或者在斑时间内变化很大的数据处理中表现一般,本文提出一种将卡尔受滤波应用到姿态解算中的算法,研究了卡尔受滤波的matlab实现,并利用卡尔受滤波对采集到的四元数选行最优估计，在采集样本差异较大的情况下实现数据平滑渐变，与一般的算法相比,提高了系统的实时性与稳定性，
关键词：四元数卡尔曼滨波matlab导航算法
文赋标识码：A
中图分类号:TN967.2
四轴飞行器是微型飞行器的其中一种，也是一种智能机器人。它利用有四个旋翼作为飞行引擎来进行空中飞行，它的尺寸较小、重量较轻，适合携带和使用的无人驾驶飞行器一样能够携带一定的任务载荷，具备自主导航飞行能力，在复杂、危险的环境下完成特定的飞行任务。飞行器姿态确定问题是飞行器导航中的关键性问题之，常用的姿态误差模型主要有欧拉角模型、等效旋转失量、四元数模型。其中四元数模型应用最广，然而由于目前主流的MEMS传感器(如mpu6000、6050、9050)只是将传感器测量到的数据输出，导致了量测噪声无法解决的间题，对此有人提出了融合滤波、互补滤波算法，改善了四元数量测噪声误差，但是这种算法本质上并没有解决噪声误差问题，并且反应速度稍慢，实时性不好。针对此，本文给出了一种卡尔曼滤波与四元数结合的算法，意在通过利用卡尔曼滤
波使系统的实时性得到提高。 1四元数与欧拉角的转换
1.1欧拉角到四元数的转换
[(z / )us(z / )us(z /)s+ (z / 小)so3(z / )so (z / )soa]( / )us(z / )u)s(z / )s0 ( / )s00( / )s00 ( / )us
9
(z / )us(z / 0)so3(z / )uS+ (z / )s0(z / 0)s(z / )s03[(/)s00(/ )u(z/ )(/)us( / )80 (z / )s00
1.2四元数到欧拉角的转换
aretat
o
(z+x)z 1-2(x*+y )
arcsin(2(wy zx))
2(wz+xy)
arcta
2卡尔曼滤波
12(y° +2*)
2.1卡尔受滤波5个基本公式
x(k)=A x(k1)+B U(k)+W(k) Z(k)=H X(k)+V(k)
(1)(2)
式(1-2)中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量，A和B是系统参数,对于多模型系统，他们为矩阵,Z(k)是k时刻的测量值，H是测量系统的参数,对于多测量系统,H为矩阵。W(k)和 V(k)分别表示过程和测量的噪声，他们被假设成高斯白噪声(White GaussianNoise)，他们的covariance分别是Q，R(这里我们假设他们不随系统状态变化而变化)。
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。下面我们来用他们收移日期：2016-10-14
文章编号:1007-9416(2016)11-0136-02
/ww
1
Phoanass Foxasisrar tel6ya
wm
hamdene
图1卡尔曼滤波和巴特沃斯低通滤波器等滤波算法
结合他们的covariances来估算系统的最优化输出。首先我们要利用系统的过程模型，来预测下一状态的系统，假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k1)=A x(k1|k1)+B U(k) P(k[k1)=A P(k1[k1) A°+Q
X(k|k)= X(k|k1)+Kg(k) (Z(k)H X(k|k1) Kg(k)= P(k|k1) H'/(H P(k|k1) H' +R)
(3)(4)(5)(6)
P(k|k)=(IKg(k) H)P(k|k1)
(7)
式(3)中,x(k|k-1)是利用上—状态预测的结果,x(k-1k-1)是上一状态最优的结果，Uk)为现在状态的控制量，如果没有控制量，它可以为0,式(4)中,P(kk-1)是x(kk-1)对应的covariance,P(k-1[k-1)是x(k-1[k-1)对应的covariance,A"表示A的转置矩阵,Q是系统过程的covariance。式(3-4)就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。式(5)其中Kg为卡尔曼增益(Kalman Gain)。式(6)已经得到了k状态下最优的估算值x(kk),但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新 k状态下x(kk)的covariance。式(7)中I为1的矩阵,对于单模型单测量,I=1,当系统进人k+1状态时,P(k|k)就是式子(4)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。卡尔受滤波器的原理基本描述了,式(3-6)和式(7)就是他的5个基本公式。
2.2卡尔曼滤波的matlab的实现我们假设状态的控制量u(k)为零：
z1[0.7854,0.7854,0.7854,0.7854,0.7854,0.7854,0.7854, 0.7854,0.7854,0.7854],
作者简介：于晓磊(1996一),男，山东蓬策人,哈尔滨理工大学本科在读,研究方向：卡尔受滤波组合导航算法
方方数据