[发明专利]一种姿态估计方法

权利要求书

1.一种姿态估计方法，其特征在于，包括以下步骤：

步骤一：令物体处于静止状态，并检测其姿态角，然后根据姿态角换算出其四元数初始化一个长度为L的队列A，并令该队列为空；初始化融合权重φ，令其为0.5；

步骤二：采集陀螺仪数据w＝[w_x,w_y,x_z]′，计算四元数向量的预测估计如下：

其中，T表示采样时间间隔，I₄表示4×4单位向量，

步骤三：读取加速度计数据a＝[a_x,a_y,a_z]′，并加入到队列A中，然后计算检测函数f，如下

其中，表示加速度计本体白噪声方差，g为重力加速度，l(·)表示取二范数；计算检测函数的理论方差如下：

接着，判断是否有如果是，则令否则，令

步骤四：利用a求取四元数，得到加速度计引出的四元数测量值q_a，如下：

其中，

步骤五：读取磁力计数据m＝[m_x,m_y,m_z]′，利用m求取四元数，得到磁力计引出的四元数测量值q_m，如下：

其中h＝[h_x,h_y,h_z]′，h可以通过计算出，R(q)为旋转矩阵，具体为：

步骤六：根据q_a和q_m计算四元数观测向量q_o，如下：其中符号定义为：

其中，ρ＝[ρ₀,ρ₁,ρ₂,ρ₃]，为两个四元数；

步骤七：计算四元数的最终估计如下并对进行归一化得到并令

步骤八：重复步骤二至步骤七，即可得到每个时刻的四元数的最终估计，从中可以换算出姿态角。