一种自适应卡尔曼滤波姿态估计算法的制作方法
本发明涉及以微机电惯性测量单元(mems-imu)为核心器件的姿态测量系统，尤其是一种自适应卡尔曼滤波姿态估计算法。
背景技术：
本算法是基于mems-imu的姿态测量系统，利用mems加速度计和mems陀螺仪输出进行数据融合，得到姿态信息的最优估计。在载体处于静止或者匀速运动状态时，加速度计可以准确的测量重力加速度，与陀螺仪输出进行融合可以获得较高的姿态测量精度。当载体有运动加速度时，加速度计输出包含两部分：重力加速度和载体运动加速度，且不能将两种加速度分离开来，导致无法以重力加速度为参考进行姿态解算。即可以认为运动加速度为干扰加速度，使得系统姿态误差增大。因此，有必要对如何有效的避免运动加速度的干扰，使得载体有运动加速度时，依然具有较高的姿态测量精度进行研究。
技术实现要素：
本发明的目的在于提高系统对运动加速度干扰的适应能力，提供一种的自适应卡尔曼滤波姿态估计算法，在载体处于不同动态条件下，滤波器可以自动调整相关参数，实现姿态信息的最优估计。
本发明解决其技术问题是采取以下技术方案实现的：
一种自适应卡尔曼滤波姿态估计算法，其特征在于：具体包括以下步骤：
⑴建立卡尔曼滤波方程；
⑵载体运动状态检测；
⑶卡尔曼滤波一步预测；
⑷陀螺仪量测更新；
⑸加速度计量测更新；
⑹加速度计量测噪声动态调节。
而且，所述步骤⑴中离散系统卡尔曼滤波器基本方程为：
其中，状态量x＝[θγωxωyωz]t；
量测量z＝[axayazgxgygz]t；
状态转移矩阵
量测矩阵
系统噪声矩阵e{wkwkt}
量测噪声矩阵e{vkvkt}
而且，所述步骤⑵，设mems陀螺仪输出角速度为gi＝[gx,igy,igz,i]t,i＝1,2,k，mems加速度计输出加速度为a＝[ax,iay,iaz,i]t,i＝1,2,k；
系统合角速度为
设系统当前状态为s，s＝1表示系统处于动态；s＝0表示系统处于静态或者匀速运动状态，系统运动状态判断规则如下：
若
而且，所述步骤⑶，卡尔曼滤波器参数初始值x0＝05×1，p0＝05×5，系统状态一步预测方程为：
而且，所述步骤⑷，利用陀螺仪输出g＝[gxgygz]t进行量测更新，更新方程如下：
其中，
而且，所述步骤⑸，利用加速度计输出a＝[axayaz]t进行量测更新，更新方程如下：
其中，
而且，所述步骤⑹，加速度计量测噪声计算公式如下：
若s＝0，则α1＝0.5，α1＝0.5；
若s＝1，则α1＝0.1，α1＝2.0；
其中，
本发明的优点和积极效果是：
本发明综合利用mems-imu输出的加速度和角速度信息，进行载体运动状态检测，根据检测结果动态调节加速度计量测噪声阵，使滤波器处于最优状态。即使系统有运动加速度时，依然保持姿态的最优估计，保证系统在不同运动状态下均具有较高的姿态测量精度，有效的提高了系统姿态测量精度。
附图说明
图1为本发明姿态股计算法流程图。
具体实施方式
下面结合附图并通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。
一种自适应卡尔曼滤波姿态估计算法，包括以下步骤：
⑴建立卡尔曼滤波方程
已知离散系统卡尔曼滤波器基本方程为：
其中，状态量x＝[θγωxωyωz]t；
量测量z＝[axayazgxgygz]t。
状态转移矩阵
量测矩阵
系统噪声矩阵e{wkwkt}
量测噪声矩阵e{vkvkt}
⑵载体运动状态检测
设mems陀螺仪输出角速度为gi＝[gx,igy,igz,i]t,i＝1,2,k，mems加速度计输出加速度为a＝[ax,iay,iaz,i]t,i＝1,2,k；系统合角速度为
设系统当前状态为s，s＝1表示系统处于动态；s＝0表示系统处于静态或者匀速运动状态。
系统运动状态判断规则如下：
若
⑶卡尔曼滤波一步预测卡尔曼滤波器参数初始值x0＝05×1，p0＝05×5，系统状态一步预测方程为：
⑷陀螺仪量测更新利用陀螺仪输出g＝[gxgygz]t进行量测更新，更新方程如下：
其中，
⑸加速度计量测更新
利用加速度计输出a＝[axayaz]t进行量测更新，更新方程如下：
其中，
⑹加速度计量测噪声动态调节加速度计量测噪声计算公式如下：
若s＝0，则α1＝0.5，α1＝0.5；
若s＝1，则α1＝0.1，α1＝2.0；
其中，
尽管为说明目的公开了本发明的实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例和附图所公开的内容。
- 还没有人留言评论。精彩留言会获得点赞！