基于EKF的IMU姿态结算
来源:互联网 发布:网络在线客服做什么的 编辑:程序博客网 时间:2024/05/29 14:42
EKF IMU
标签: IMU
说明
- 具体代码实现:https://github.com/Xiarain/Rain_IMU
- 代码实现部分分为三个部分,基于A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU中论文的实现,基于EKF-IMU算法的实现,基于ESKF-IMU算法的实现。
RAIN IMU姿态解算规则说明
四元数旋转矩阵(从body坐标系到world坐标系):
欧拉角符号说明:
- 俯仰角,绕
X 轴旋转Pitch角,θ - 偏航角,绕
Y 轴旋转Yaw角,ψ - 滚转角,绕
Z 轴旋转Roll角,ϕ - 定义欧拉角旋转顺序:先偏航角、后俯仰角、最后滚转角。
数值积分方法选择欧拉方法:
状态量选择:
实现步骤:
step1:
初始化四元数:
初始化
step2:
状态观测量:
需要注意的是这里的状态观测量需要对加速度和磁力计进行归一化,这个非常重要。
step3:
状态转移方程:
将四元数状态转移方程展开则为如下公式:
需要进行对状态量中的四元数中归一化。
step4:
状态转移方程矩阵表示:
求状态转移方程的雅克比矩阵:
计算协方差矩阵:
step5:
测量方程:
重力在世界坐标系下归一化可以得到重力参考值
加速度计可以测量重力在机体坐标系下各轴的分量。
加速度测量模型:
由于加速度计测量原理的构造,加速度测量模型测量的加速度值是以单位重力
为了消除环境因素的干扰,需要进行坐标变换;因为磁倾角的存在,地球的磁场强度可以认为是由水平和垂直两部分组成的。一个加速度计可以提供一个姿态的参考,而且可以用来补偿测量地球磁场强度中倾斜错误。将传感器数值从传感器参考系转换到地球参考系。
对
step6:
计算卡尔曼增益:
step7:
更新后验状态:
step7:
更新后验误差协方差矩阵:
注意事项:
1.大型矩阵的构造应该是尝试使用基础矢量或者矩阵的数学构造的方式,而不是通过人为的方式来填充,这样太容易出现错误,而且很不容易查找错误。
2.不知道是不是因为数据集的关系,在加速度计和磁力计的观测矩阵中,加速度计的观测矩阵与理论分析中相差一个负号,而磁力计是与理论分析是一样的。
阅读全文
0 0
- 基于EKF的IMU姿态结算
- 基于EKF的姿态解算
- 基于四元数的扩展卡尔曼(EKF)滤波器四旋翼姿态解算详解-2.EKF介绍
- 基于四元数的扩展卡尔曼(EKF)滤波器四旋翼姿态解算详解-1.KF介绍
- 基于四元数的扩展卡尔曼滤波器(EKF)姿态估计文献小结
- 姿态结算中旋转计算的基础
- 姿态结算相关-----姿态的表示和传感器
- PIXHAWK 飞控中的EKF姿态估计的欧拉角求解
- IMU姿态融合算法
- imu姿态融合基础
- 使用EKF算法处理IMU数据
- 基于matlab的双目+IMU标定
- 无人机姿态融合——EKF
- 姿态结算----梯度下降法
- 基于姿态传感器的下肢姿态测量演示系统
- 姿态结算相关----梯度下降法
- 基于特征匹配的摄像头姿态估计
- 基于深度图像的头部姿态估计
- android 夜间模式
- 动态改变div位置
- struts2中各个jar包作用
- 深拷贝与浅拷贝
- WPF下 使用它tabcontrol控件 时遇到的指定控件获得焦点的问题
- 基于EKF的IMU姿态结算
- CocoaPod的使用
- :eq()选择器
- Http2.0你知道多少?
- KMP模板 POJ 3461 KMP模板讲解
- CentOS搭建PySpider爬虫服务
- Android实现全屏显示、去掉标题
- RPC框架简述
- SAP系统中常用的FI报表