[发明专利]基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法

权利要求书

1.基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法，其特征在于，所述方法包括如下步骤：

步骤一：利用加速度计和陀螺仪采集关于X轴的数据，获得加速度和角速率；

步骤二：对获得的加速度和角速率进行数字滤波，滤除在传输过程中产生的高频干扰；

步骤三：将滤波后的加速度转换成角度；

步骤四：根据转换的角度，代入温度补偿因素，得到补偿后的角度；

步骤五：将补偿后的角度和滤波后的角速率进行卡尔曼滤波，并进行数据融合，获得水平角度的最优估计值。

2.根据权利要求1所述的基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法，其特征在于，所述步骤五的过程包括：

步骤五一：设置参数的初值：

矩阵A=1-dt01,]]>t表示当前时刻，dt表示积分，矩阵B=dt0,]]>过程噪声的协方差矩阵Q=qa00qg,]]>q_a表示加速度计的测量协方差，q_g表示陀螺仪的测量协方差，R＝[r_a]，r_a表示测量误差的协方差，水平角度估计值的协方差初值P(0/0)=0000,]]>水平角度估计初值X(0/0)=00;]]>

步骤五二：预测水平角度估计值协方差P(t|t-1)：

P(t|t-1)＝AP(t-1|t-1)A^T+Q；

P(t|t-1)为X(t|t-1)的协方差；

步骤五三：根据P(t|t-1)，计算卡尔曼增益Kg(t)：

Kg(t)＝P(t|t-1)H^T/[HP(t|t-1)H^T+R]；

观测矩阵H＝[10]；

步骤五四：预测水平角度估计值X(t|t-1)：

X(t|t-1)＝AX(t-1|t-1)+BU(t)；

X(t|t-1)表示根据t-1时刻的预测t时刻的水平角度估计值，U(t)表示t时刻的控制量；

步骤五五：根据当前时刻的角速率测量值Z(t)，结合X(t|t-1)和Kg(t)，获得水平角度的最优估计值X(t|t)：

X(t|t)＝X(t|t-1)+Kg(t)[Z(t)-HX(t|t-1)]；

步骤五六：根据X(t|t)，获得协方差P(t|t)：

P(t|t)＝[I_n-Kg(t)H]P(t|t-1)；

I_n表示n阶单位矩阵，即对角线元素全为1的矩阵；

转入步骤五二，预测下一时刻的协方差。