四旋翼无人机设计与制作
K(k)=P(k|k-1) * H(k) / (H(k) * P(k|k-1) * H(k) + R(k)) (4-3) 其中，K(k)表示卡尔曼增益；R(k)表示k时刻观测过程的covariance，即对测量的信任程度；H(k)表示观测矩阵。
更新估计：
X(k|k)=X(k|k-1)+ K(k) * (Z(k) – H(k) * X(k|k-1)) (4-4) P(k|k)=(1- K(k)* H(k)) * P(k|k-1) (4-5) 其中，Z(k)表示k时刻的观测值，I为单位矩阵。 4.2.3 电子罗盘信号处理
电子罗盘信号非常容易受到高频干扰，在系统应用中，应当过滤系统的高频噪声，最好采用去极值滤波算法[19]。去极值滤波就是将连续测量的n个采样值，按照数据的大小顺序进行排序，去掉最大值和最小值后对剩下的n-2个数值计算求取平均值，这种方法是最常用的滤波算法[20]。
4.3 坐标系
坐标系是描述物体在空间的相对位置和运动规律的，而导航技术就是为了确定载体的空间位置。只有选定参考坐标系，才能对系统的运动进行描述。不同的坐标系下载体的描述规律和运动形式也是不同的，选择合适的坐标系是非常重要的[21]。 目前比较常用的坐标系有： (1)地理坐标系(g系)
坐标系和地球固连，其原点位于地球球心，通常选取东北天坐标系，即坐标Xg轴指向水平东方，Yg轴指向水平北方，Zg轴垂直于当地水平面，沿当地垂线向上[22]。 (2)导航坐标系(n系)
一般选取当地地理坐标系作为导航坐标系，坐标Xn轴指向地理东方，Yn轴指向地理北方，Zn轴垂直于当地水平面，沿当地垂线向上[23],如图4.2所示。 (3)载体坐标系(b系)
载体坐标系，原点位于机体的质心位置，通常选取右前上坐标系，其Xb轴沿机 体横轴向右，Yb轴沿机体纵轴向前，Zb轴沿机体竖轴向上，如图4.2所示。
–17–
四旋翼无人机设计与制作
图 4.2 载体坐标系和导航坐标系
4.4 姿态角定义
姿态角也是我们常说的欧拉角，是根据导航坐标系和载体坐标系之间的关系来定义的。欧拉角是飞行器的三个姿态角即俯仰角、横滚角和偏航角，根据欧拉旋转定律可以用三次旋转使得飞行器本身的坐标系与地理参考系重合，每一次的旋转以机体坐标系的x、y、z轴中的一个坐标轴来转动，转过的角度即为欧拉角，三次坐标的变换矩阵相乘的积就是欧拉姿态矩阵[24]，形如4-1：
00??1?cos?0?sin???cos?sin?0??C??01?C???sin?cos?0? (4-6) Cx??0cos?sin?0y????z?????01??0?sin?cos????sin?0cos????0?最终的姿态矩阵与这三次转动的先后顺序是有关系的，通常我们都按照Z-X-Y轴的顺序。定义机体绕本体系x轴转动的角度为俯仰角?；机体绕本体系y轴转动的角度为横滚角?；机体绕本体系z轴转动的角度为航向角?；三个角当转动方向与旋转轴符合右手定则为正方向[25]。俯仰角?、横滚角?和航向角?合称欧拉角。得到如下姿态矩阵4-2：
–18–
四旋翼无人机设计与制作
00??cos?sin?0??cos?0?sin???1??0cos?sin????sin?cos?0?bCn=CyCxCz=?010???????01??sin?0cos?????0?sin?cos?????0? (4-7)
?cos?cos??sin?sin?sin?cos?sin??sin?sin?cos??sin?cos???=??cos?sin?cos?cos?sin?????sin?cos??cos?sin?sin?sin?sin??cos?cos?sin?cos?cos???4.5 四元数姿态解算算法
四元数其实是 1个单位实数和3个虚数单位i、j 和 k 的线性组合，一般可表示为d + ai + bj + ck， a、b、c、d代表实数[26]。
四元数乘法运算关系如下：
i?i??1，j?j??1，k?k??1i?j?k， j?k?i， k?i?j j?i??k，k?j??i，i?k??j式中，?表示的是四元数的乘法。 四元数与姿态矩阵之间的关系：
设有参考坐标系R，坐标轴X0、 Y0、 Z0，坐标轴方向的单位向量为i0、j0 、
k0[27]。刚体相对于坐标系R作定点转动，定点为O，把坐标系b与刚体固联，b系的坐标轴为x、y、z，坐标轴方向的单位向量为i、j 、 k量描述了刚体的空间角位置[29]。
[28]
。假设初始时刻b系与R
系重合。在刚体上任取一点A，从 O点向该点引向量OA,如图4.3所示。则该位置向
图4.3 刚体的等效转换
–19–
四旋翼无人机设计与制作
设刚体以ω= ?xi??yj??zk相对R系旋转，初始时刻位置处于OA=r，经过时间t后位置向量处于OA'?r'。根据欧拉定理，刚体从A位置转到A'位置的转动可等效成绕瞬轴u转过?角一次完成[30]。这样，位置向量做圆锥运动，A和A'位于同一圆上，r和r'位于同一圆锥面上。
在圆上取一点B，是?AO'B?90?，由图4.3可得。
OO'?(r?u)u O'A?r?OO'?r?(r?u)u
O'B?u?O'A?u?r?(r?u)u?u?u?rO'A'?OA'cos??OB'sin?
?rcos??(r?u)ucos???u?rsin?所以
r'=O'O'?O'A'?rcos??(1?cos?)?(r?u)u?u?rsin?由三重矢量计算公式：
u?(u?r)?u(u?r)?(u?u)r
?(r?u)u?r
即
(r?u)u?r?u?(u?r)
所以
r'?rcos??(1?cos?)[r?u?(u?r)]?u?rsin??r?u?rsin??(1?cos?)u?(u?r)将上式向R系内投影：
r'R?rR?(u?r)Rsin??(1?cos?)[u?(u?r)]R
记
?r'x??rx??l????，uR??m? rr'R??r'y?，rR??y?????'????rz???n???rz?根据叉乘关系表达式：
–20–