状态量尤轉=I _ ρ . .：，状态矩阵 I gyro Dias 1 一 dt dt 沒=Θ1 ，控制矩阵c= 〇
[0086] 对比式(2)和式(4)可得，系统测量量Y(k) = [angle_acc(k)]，测量矩阵H=[l 0];
[0087] S22:根据陀螺仪的测量数据，由上一时刻陀螺仪测量数据计算的姿态角度预测当 前时刻的姿态角度，计算公式如下：
[0088] angle_gyro(k)=angle_gyro(k_l) + [w_gyro(k_l)_gyro_bias] · dt (5)
[0089] S23:根据加速度计测量数据计算得到的姿态角度与步骤S22中陀螺仪测量数据计 算的姿态角度，计算姿态角度误差，计算公式如下：
[0090] ang1e_err(k)= ang1e_acc(k)-ang1e_gyro(k) (6)
[0091] 在式（6)中，angle_acc(k)为k时刻加速度计测量数据计算的第二姿态角度， angle_gyro(k)为步骤S22中计算的第一姿态角度，angle_err(k)为k时刻的上述两者的误 差值；
[0092] S24:采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声Μ的协 方差Ν，计算公式如下：
[0093] (7>
[0094] (8)
[0095] 其中：Q_acc为加速度计的过程噪声协方差，Q_gyro为陀螺仪的过程噪声协方差， Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_gyro)为陀螺仪 测量数据计算的姿态角度的方差；
[0096] S25:计算卡尔曼滤波的状态量X' (k)对应的协方差R(k)，计算公式如下：
[0097] R(k)=S · R(k-l) · ST+Q (9)
[0098] 式(9)中S为控制矩阵，ST为控制矩阵S的转置，Q为过程噪声P的协方差；
[0099] S26:计算卡尔曼滤波增益Kg，计算公式如下：
[moo]
(1())
[0101] 式(10)中Η为测量矩阵，Ητ为测量矩阵Η的转置，N为测量噪声Μ的协方差；
[0102] S27:计算修正后的k时刻的卡尔曼滤波的状态量Xlk)，计算方式如下：
[0103] X7 (k)=X7 (k-l)+Kg(k) · angle_err(k) (11)
[0104] S28:将k时刻的卡尔曼滤波的状态量X' (k)中的第一行第一列的值提取出来，即为 根据卡尔曼滤波算法融合所述第一姿态角度和所述第二姿态角度而计算的所述第三姿态 角度，以angle_Kalman(k)表示，计算公式如下：
[0105] angle_Kalman(k) =X7 (k)(1,1) (12)
[0106] S29:为了使卡尔曼滤波算法持续不断的运行，对下一时刻的卡尔曼滤波的状态量 V (k+Ι)对应的协方差R(k+1)进行更新，计算公式如下：
[0107] R(k+l)=R(k)-Kg(k) · Η · R(k) (13)
[0108] 其中，为了使互补卡尔曼滤波算法开始工作，在进行卡尔曼滤波算法之前需要设 定卡尔曼滤波算法的状态量X' (k)的初始值X' (0)及其对应的协方差R(k)的初始值R(0)，这 两个值都可以设定为任意值，因为随着卡尔曼滤波算法的运行，卡尔曼滤波算法的状态量 X' (k)会逐渐收敛，其中，对于R(0)的值一般不设为零值，由于设为零值时可能使卡尔曼滤 波算法认为给定的X10)是最优的，从而使算法不能收敛，因此本发明中将R(〇)设定为非零 任意值。
[0109] S3:利用陀螺仪测量数据计算的第一姿态角度、加速度计测量数据计算的第二姿 态角度及卡尔曼滤波算法融合两种传感器测量数据而计算的第三姿态角度，根据三者方差 赋予相应的姿态角度权重，方差大者，在互补卡尔曼滤波算法中所占比重小。基于上述思 想，设计互补卡尔曼滤波算法，计算过程包括如下步骤：
[0110] S31:根据陀螺仪测量数据计算的所述第一姿态角度和加速度计测量数据计算的 所述第二姿态角度，求出所述第一姿态角度和所述第二姿态角度的权重，计算公式如下：
[0111]
(14.)
[0112] 其中：B_gyr〇为陀螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角 度中的权重，Var(angle_acc)为加速度计测量数据计算的姿态角度的方差，Var(angle_ gyro)为陀螺仪测量数据计算的姿态角度的方差；
[0113] S32:计算根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔曼滤波 算法中的权重，计算公式如下：
[0114]
15)
[0115]其中:A_Kalman为根据卡尔曼滤波算法计算得到的所述第三姿态角度在互补卡尔 曼滤波算法中的权重，Var(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差； [0116] S33:计算互补卡尔曼滤波算法融合三个姿态角度而计算的融合姿态角度，计算公 式如下：
[0117] angle_ComKalman(k)=A_Kalman · angle_Kalman(k)
[0118] +(l-A_Kalman) · B_gyro · angle_gyro(k) (16)
[0119] +(l_A_Kalman) · (l_B_gyro) · angle_acc(k)
[0120]其中：angle_ComKalman(k)为根据互补卡尔曼滤波算法计算的融合姿态角度。 [0121]为了验证本发明，采用如图2所示的基于互补卡尔曼滤波算法计算融合姿态角度 的方法的实验系统来进行计算，以STM32F103RB单片机为控制器，传感器采用MAX21100单芯 片"3轴陀螺仪+3轴加速度计"惯性测量单元（IMU)。该系统还包括电源稳压电路（采用 LT8620降压型开关稳压器）、晶振电路(晶振频率为8MHz)、复位电路、输出短路保护电路(采 用LT3579EFE具故障保护功能的升压/负输出转换器）、D/A转换滤波电路、RS-232线路驱动/ 接收电路、CMSIS-DAP仿真下载器、输出信号放大电路等。传感器固定连接在微波天线上，利 用振动台来产生振动信号，将传感器信号通过RS232接口传给STM32单片机，STM32单片机将 采集到的陀螺仪测量数据和加速度计测量数据输出到PC机上并保存，后续利用MATLAB编程 处理数据，对本发明的一种基于互补卡尔曼滤波算法计算的融合姿态角度的方法进行验 证。验证结果如图3和图4所示，其中图3是在静态条件下，本发明的互补卡尔曼滤波算法与 卡尔曼滤波算法计算的姿态角度的对比图，其中采用卡尔曼滤波算法的方差为0.0010194， 而采用本发明的互补卡尔曼滤波算法的方差为〇. 0007526，方差大大下降，精度得到提高； 图4是在动态条件下，本发明的互补卡尔曼滤波算法与卡尔曼滤波算法计算的姿态角度的 对比图，采用本发明的互补卡尔曼滤波算法计算的融合姿态角度比卡尔曼滤波算法计算的 姿态角度的测量精度提高了13.3%。
[0122] 本发明的基本思路是:首先搭建微波天线姿态角度测量系统，对卡尔曼滤波算法 中参数一一过程噪声协方差和测量噪声协方差的计算方法，提出采用计算姿态角度方差的 方法进行描述;其次，根据互补滤波原理，提出融合陀螺仪测量数据计算的第一姿态角度、 加速度计测量数据计算的第二姿态角度和卡尔曼滤波算法融合两种传感器测量数据得到 的第三姿态角度的互补卡尔曼滤波算法;最后，利用测量系统采集数据，基于MATLAB编写互 补卡尔