陀螺仪c语言算法,陀螺仪传感器建模与卡尔曼姿态解算

陀螺仪传感器建模与卡尔曼姿态解算

说到姿态解算，目前用到最多的应该是“六轴加速度陀螺仪传感器+卡尔曼滤波算法”。网上的代码和卡尔曼滤波算法的教程很多，但大多数都是分开的两块，很少有将其结合在一起的。

本篇博客重点解释陀螺仪加速度计传感器在姿态解算方面如何建立模型，以及如何编写相应的代码。

一.模型建立

对于角度计算，我们有如下公式：

其中为陀螺仪测得的角速度

由于现实中陀螺仪存在漂移，所以对上式进行修正如下

其中Gyro为陀螺仪静态漂移

由此我们建立状态方程：

状态量：

测量量：

二.卡尔曼5个公式

三。编程(C语言)

至此，陀螺仪加速度计的建模和编程工作全部完毕，代码摘至网络，本人只是将其与书本中公式结合，希望对读者有所帮助

附：(完整代码)

`//Kalman滤波//

//-------------------------------------------------------

static float Q_angle=0.001, Q_gyro=0.001, R_angle=5, dt=0.004;

//Q增大，动态响应增大

static float Pk[2][2] = { {1, 0}, {0, 1 }};

static float Pdot[4] ={0,0,0,0};

static float q_bias=0, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

//-------------------------------------------------------

void Kalman_Filter(float angle_m,float gyro_m)

{

Angle+=(gyro_m-q_bias) * dt; ///预测值

Pdot[0]=Q_angle - Pk[0][1] - Pk[1][0];

Pdot[1]=- Pk[1][1];

Pdot[2]=- Pk[1][1];

Pdot[3]=Q_gyro;

Pk[0][0] += Pdot[0] * dt;

Pk[0][1] += Pdot[1] * dt;

Pk[1][0] += Pdot[2] * dt;

Pk[1][1] += Pdot[3] * dt;

angle_err = angle_m -Angle;///测量值-预测值

PCt_0 = Pk[0][0];

PCt_1 = Pk[1][0];

E = R_angle + PCt_0;

K_0 = PCt_0 / E; ///卡尔曼增益

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = Pk[0][1];

Pk[0][0] -= K_0 * t_0;

Pk[0][1] -= K_0 * t_1;

Pk[1][0] -= K_1 * t_0;

Pk[1][1] -= K_1 * t_1;

Angle+= K_0 * angle_err; ///最优角度=预测值+卡尔曼增益*(测量值-预测值)

q_bias+= K_1 * angle_err;

Angle_Speed = gyro_m-q_bias;1

文章来源: blog.csdn.net，作者：生于理想✔，版权归原作者所有，如需转载，请联系作者。

原文链接：blog.csdn.net/stcstm/article/details/112794152

陀螺仪c语言算法,陀螺仪传感器建模与卡尔曼姿态解算相关推荐

陀螺仪c语言算法,陀螺仪c程序.docx
陀螺仪c程序飞思卡尔XS128的程序/****************************************************************Code Warrior 5.9 ...
AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析)
转载链接:http://www.51hei.com/bbs/dpj-92911-1.html AHRS俗称航姿参考系统,AHRS由加速度计,磁场计,陀螺仪构成,AHRS的真正参考来自于地球的重力场和地 ...
MiniFly微型四轴学习开发日志（三）——关于姿态解算的相关算法
目录大致内容介绍欧拉角与四元数互补滤波算法的姿态解算一些疑问大致内容介绍之前两篇日志当中提到了关于角度-角速度串级PID与定高Z轴PID配合对四轴进行姿态矫正的大概算法.但是对于测量而来的 ...
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜上＞
这是"四轮车驱动控制"系列,分多个小节来介绍: 1. 八.四轮车驱动开发之一:正/逆向运动学分析 2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机 3. 十.四轮车驱动开发之 ...
十三. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜中＞
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(上) 十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中) 十四.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算 ...
十四. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜下＞
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(上) 十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中) 十四.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算 ...
姿态解算知识（三）-陀螺仪加速度计6轴数据融合
这么久的惯导总算是没白看,加上一篇博客的指点,这两天把Mahony的九轴数据融合算法看懂了.可惜第二版硬件还没到,磁力计用不了,没法验证效果~今天先总结下陀螺仪和加速度计的六轴数据融合. 版权声明原 ...
android 陀螺仪滤波_Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验
Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验版权声明:本文为博主原创文章,未经博主允许不得转载. 2019年3月20日发布实例效果输出效果: 首先看看本例程XYZ轴的输出效果 ...
基于STM32F407四旋翼无人机 --- 姿态解算讲解（四元数）（叉积法融合陀螺仪数据和加速度数据）（五）
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五) 姿态解算姿态解算定义欧拉角四元数四元数性质方向余弦矩阵四元数方向余弦矩阵叉积法融合陀螺仪数据和加速度数据叉积运算一阶龙 ...
详解几种飞控的姿态解算算法
姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...

陀螺仪c语言算法,陀螺仪传感器建模与卡尔曼姿态解算

陀螺仪c语言算法,陀螺仪传感器建模与卡尔曼姿态解算相关推荐

最新文章

热门文章