四元数姿态解算详细步骤
2024-05-10 02:30:32
- 获取陀螺仪和加速度计原始数据
short gryox,gyroy,gyroz;
short accx,accy,accz;
void IMU_GetData(void)
{MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);MPU_Get_Accelerometer(&accx,&accy,&accz);}
- 定义变量
#define RtA 57.295779f //弧度->角度
#define AtR 0.0174533f //角度->弧度
#define Acc_G 0.0011963f
#define Gyro_G 0.0610351f
#define Gyro_Gr 0.0010653f #define Kp 18.0f
#define Ki 0.008f
#define halfT 0.008f float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle[3]={0};//最终角度
- 函数声明
/*************************
*函数名：IMU_Update
*输入：陀螺仪，加速度计原始数据（short类型)
*************************/
void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);- 函数实现
void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
{float ax=accx,ay=accy,az=accz;float gx=gyrox,gy=gyroy,gz=gyroz;float norm;float vx, vy, vz;float ex, ey, ez;float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;float q1q1 = q1*q1;float q1q3 = q1*q3;float q2q2 = q2*q2;float q2q3 = q2*q3;float q3q3 = q3*q3;if(ax*ay*az==0)//此时任意方向角加速度为0return; gx *= Gyro_Gr;gy *= Gyro_Gr;gz *= Gyro_Gr;norm = sqrt(ax*ax + ay*ay + az*az); ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w) vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (ay*vz - az*vy) ; ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; // integrate quaternion rate and normalise q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;// angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; // yawANgle[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitchANgle[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; //roll}- IMU.h
//IMU.h
#ifndef _IMU_H
#define _IMU_H
/*************************
*函数名：IMU_Update
*输入：陀螺仪，加速度计原始数据（short类型)
*************************/
void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);#endif
- IMU.c
//IMU.c
#include "IMU.h"
short gryox,gyroy,gyroz;
short accx,accy,accz;
void IMU_GetData(void)
{MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);MPU_Get_Accelerometer(&accx,&accy,&accz);}#define RtA 57.295779f //弧度->角度
#define AtR 0.0174533f //角度->弧度
#define Acc_G 0.0011963f
#define Gyro_G 0.0610351f
#define Gyro_Gr 0.0010653f #define Kp 18.0f
#define Ki 0.008f
#define halfT 0.008f float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle[3]={0};//最终角度void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
{float ax=accx,ay=accy,az=accz;float gx=gyrox,gy=gyroy,gz=gyroz;float norm;float vx, vy, vz;float ex, ey, ez;float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;float q1q1 = q1*q1;float q1q3 = q1*q3;float q2q2 = q2*q2;float q2q3 = q2*q3;float q3q3 = q3*q3;if(ax*ay*az==0)//此时任意方向角加速度为0return; gx *= Gyro_Gr;gy *= Gyro_Gr;gz *= Gyro_Gr;norm = sqrt(ax*ax + ay*ay + az*az); ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w) vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (ay*vz - az*vy) ; ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; // integrate quaternion rate and normalise q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;// angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; // yawANgle[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitchANgle[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; //roll}
四元数姿态解算详细步骤相关推荐
- 四元数姿态解算及多传感器融合详细解析
代码路径ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp ...
- 一阶高通滤波+二阶Mahony滤波的四元数姿态解算
此次实验我使用icm20602进行 icm20602输出有以下特点: 3轴陀螺仪可选量程有±250dps,±500dps,±1000dps,±2000dps.(dps:degrees per seco ...
- 四元数姿态解算算法基础
文章目录 姿态的表示方法 四元数表示姿态的物理意义 使用四元数进行载体姿态更新方程 四元数微分方程 四元数初始值确定 姿态的表示方法 载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数. 欧 ...
- 四元数姿态解算c语言例程_（21）用四元数插值来对齐IMU和图像帧
最近一直在外实习,好久没更新博文了,实在罪过哈. 面临秋招,亚历山大,宝宝想哭,但要坚强. 前不久看到一句话,很有感触,送给一起秋招的小伙伴拉: 不要等到准备充足了才去开始,放出去溜溜吧拉1 说正事哈 ...
- 姿态解算 四元数、方向余弦、欧拉角、Mahony滤波、四轴
姿态解算 四元数.方向余弦.欧拉角.Mahony滤波 说明:本文只是做了一些总结,需要一些对这方面的基础概念的了解. 一般人千万不要试图去深入探讨四元数 1. 方向余弦矩阵 方向余弦矩阵是使用欧拉角( ...
- 四旋翼无人机飞控系统设计(姿态解算)
姿态解算 姿态传感器读出加速度和角速度,而对一个系统的自动控制往往需要更加上层和贴近应用的的一个属性:角度.所以需要通过加速度和角速度进行数据融合转化得到姿态角度. 以MPU6050为例,姿态 ...
- 基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）
前言:本文为手把手教学飞控核心知识点之一的姿态解算--MPU6050 姿态解算(飞控专栏第2篇).项目中飞行器使用 MPU6050 传感器对飞行器的姿态进行解算(四元数方法),搭配设计的卡尔曼滤波器与 ...
- 姿态解算(用于飞行器或ROV等)的基本思路和数学原理
文章目录 (一)前言 (二)方向余弦矩阵DCM (三)四元数 (四)求四元数 更新四元数 误差消除 (五)总结 (一)前言 数学是算法的门槛 我们就用数学的角度去探寻姿态解算,不谈代码 主要是 ...
- UAV012_V2（二）：无人机姿态解算（深入篇）
写这篇博客,已经是第三次了,花了一个周,一遍遍修改,只为了理解好姿态解算并表述出来. 之前写过一篇姿态解算的博客,UAV021(四):飞控传感器数据融合与姿态估计,在小角度假设条件(俯仰角.滚转角都很 ...
最新文章
- 使用git上传代码到github
- 《iOS 6核心开发手册（第4版）》——2.1节UIControl类
- java第五章：面向对象（oop）
- Android 面试题集整理
- 中国大学MOOC 编译原理 第6讲测验
- CCNA试验-1标准acl
- 用java servlet Filter编写过滤器
- 聚簇索引与非聚簇索引的区别以及SQL Server查询优化技术
- Oracle 12C 新特性之扩展数据类型（extended data type）
- SCCM 2012系列13 操作系统播发②
- loadrunner11 中文破解版(附详细安装教程)
- 5.16 BScroll页面切换滑动失效，点击事件冒泡（未绑定click也会监听click)，transition-group动画特例，BScroll未激活不允许任何事件调用，类的实例和对象
- 物联网是什么意思？物联网概念是什么？
- qt c语言混合编程 pdf,QT中的C++技术 pdf
- matlab 2015 积分,Matlab中如何求解积分？
- 广告投放的相关名词CPM/CPT/CPC/CPD/CPI/CPS
- ★电车难题的n个坑爹变种
- 筱筱笔记：npm发包流程
- 小米前端面经(社招)
- VUE React Angular