一种自适应卡尔曼滤波姿态估计算法制造技术

本发明专利技术公开了一种自适应卡尔曼滤波姿态估计算法，具体包括以下步骤：⑴建立卡尔曼滤波方程；⑵载体运动状态检测；⑶卡尔曼滤波一步预测；⑷陀螺仪量测更新；⑸加速度计量测更新；⑹加速度计量测噪声动态调节。本发明专利技术综合利用MEMS‑IMU输出的加速度和角速度信息，进行载体运动状态检测，根据检测结果动态调节加速度计量测噪声阵，使滤波器处于最优状态。即使系统有运动加速度时，依然保持姿态的最优估计，保证系统在不同运动状态下均具有较高的姿态测量精度，有效的提高了系统姿态测量精度。

An Adaptive Kalman Filter Attitude Estimation Algorithm

【技术实现步骤摘要】
一种自适应卡尔曼滤波姿态估计算法
本专利技术涉及以微机电惯性测量单元(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表示系统处于静态或者匀速运动状态，系统运动状态判断规则如下：若且则判断系统处于静态或者匀速运动状态S＝0；若或则系统处于动态S＝1；T为MEMS-IMU采样时间。而且，所述步骤⑶，卡尔曼滤波器参数初始值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；其中，yk为加速度计量测滤波新息，Sk为yk的协方差；是以加速度计量测新息为输入的卡方分布函数，即滤波器根据加速度计量测的新息动态调节量测噪声阵R2的大小，进而调节滤波器增益Kk+1,a的大小，实现滤波器参数的自适应调整。本专利技术的优点和积极效果是：本专利技术综合利用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表示系统处于静态或者匀速运动状态。系统运动状态判断规则如下：若且则判断系统处于静态或者匀速运动状态S＝0；若或则系统处于动态S＝1；T为MEMS-IMU采样时间。⑶卡尔曼滤波一步预测卡尔曼滤波器参数初始值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；其中，yk为加速度计量测滤波新息，Sk为yk的协方差；是以加速度计量测新息为输入的卡方分布函数，即滤波器根据加速度计量测的新息动态调节量测噪声阵R2的大小，进而调节滤波器增益Kk+1,a的大小，实现滤波器参数的自适应调整。尽管为说明目的公开了本专利技术的实施例和附图，但是本领域的技术人员可以理解：在不脱离本专利技术及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本专利技术的范围不局限于实施例和附图所公开的内容。本文档来自技高网...

【技术保护点】
1.一种自适应卡尔曼滤波姿态估计算法，其特征在于：具体包括以下步骤：⑴建立卡尔曼滤波方程；⑵载体运动状态检测；⑶卡尔曼滤波一步预测；⑷陀螺仪量测更新；⑸加速度计量测更新；⑹加速度计量测噪声动态调节。

【技术特征摘要】
1.一种自适应卡尔曼滤波姿态估计算法，其特征在于：具体包括以下步骤：⑴建立卡尔曼滤波方程；⑵载体运动状态检测；⑶卡尔曼滤波一步预测；⑷陀螺仪量测更新；⑸加速度计量测更新；⑹加速度计量测噪声动态调节。2.根据权利要求1所述的自适应卡尔曼滤波姿态估计算法，其特征在于：所述步骤⑴中离散系统卡尔曼滤波器基本方程为：其中，状态量X＝[θγωxωyωz]T；量测量Z＝[axayazgxgygz]T；状态转移矩阵其中量测矩阵系统噪声矩阵量测噪声矩阵3.根据权利要求1所述的自适应卡尔曼滤波姿态估计算法，其特征在于：所述步骤⑵，设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表示系统处于静态或者匀速运动状态，系统运动状态判断规则如下：若且则判断系统处于静态或者匀速运动状态S＝0；若或则系统处于动态S＝1；T为MEMS-IMU采样时间。4.根据...

【专利技术属性】
技术研发人员：杨松普，周凌峰，侯志宁，李巍，
申请(专利权)人：中国船舶重工集团公司第七零七研究所，
类型：发明
国别省市：天津,12