AHRS算法代码：磁力计+加计+陀螺版

以下代码是上面捷联惯导算法代码的磁力计+加计+陀螺版，没仔细研究过，粗看看像是把磁阻mxyz的向量转到地理坐标系，然后用地理坐标系的正北向标准磁场向量取代变成bxyz？又转回机体坐标系变成wxyz，最后和原始磁阻测量值mxyz做向量叉积来修正陀螺。想学习的就研究研究吧！

//=====================================================================================================

// AHRS.c
// S.O.H. Madgwick
// 25th August 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion
// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation. See my report for an overview of the use of quaternions in this application.
//
// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
//
//=====================================================================================================

//----------------------------------------------------------------------------------------------------
// Header files

#include "AHRS.h"
#include <math.h>

//----------------------------------------------------------------------------------------------------
// Definitions

#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f // half the sample period

//---------------------------------------------------------------------------------------------------
// Variable definitions

float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error

//====================================================================================================
// Function
//====================================================================================================

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;

// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;

// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;

// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;

// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;

// adjusted gyroscope measurements
gx = 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 quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

AHRS算法代码：磁力计+加计+陀螺版相关推荐

UVA-12171 雕塑题解答案代码算法竞赛入门经典第二版
GitHub - jzplp/aoapc-UVA-Answer: 算法竞赛入门经典例题和习题答案刘汝佳第二版这道题目在<算法竞赛入门经典第二版>书中标注了星号,也是第一道出现星号的 ...
微软公司等数据结构+算法面试100题2010版全部出炉
微软等公司数据结构+算法面试100题2010版首次完整亮相作者:July.2010年12月6日. 更新:现今,这100题的答案已经全部整理出来了,微软 ...
Madgwick AHRS算法笔记
Madgwick AHRS算法笔记 Madgwick AHRS算法笔记引言坐标系 1. 地理坐标系 2. 载体坐标系四元数 1. 四元数基本理论 1.1 定义 1.2 运算 2. 四元数表示旋转 ...
『HTML5实现人工智能』小游戏《井字棋》发布，据说IQ上200才能赢【算法代码讲解+资源打包下载】...
一,什么是TicTacToe(井字棋) 本游戏为在下用lufylegend开发的第二款小游戏.此游戏是大家想必大家小时候都玩过,因为玩它很简单,只需要一张草稿纸和一只笔就能开始游戏,所以广受儿童欢迎. ...
九轴传感器姿态----AHRS算法开源项目推荐
1. AHRS简介九轴MEMS传感器(三轴陀螺仪+三轴加速度计+三轴磁强计)可以用来做航向和姿态参考系统,即AHRS(attitude &heading reference system), ...
《Python程序设计与算法基础教程（第二版）》江红余青松，第九章课后习题答案
推荐阅读 <Python程序设计与算法基础教程(第二版)>江红余青松全部章节的课后习题,上机实践,课后答案,案例研究文章目录例9.1~例9.53 填空题:2 思考题:3~11 上机 ...
《Python程序设计与算法基础教程（第二版）》江红余青松全部章节的课后习题，上机实践，课后答案，案例研究
(还在更新中-) 这篇博客花费了我的大量时间和精力,从创作到维护:若认可本篇博客,希望给一个点赞.收藏并且,遇到了什么问题,请在评论区留言,我会及时回复的这本书对Python的知识点的描述很详细, ...
“泰迪杯”挑战赛 - 基于用户协同过滤算法的电影推荐系统(附算法代码)
目录第 1 章绪论 1.1.研究背景 1.2.国际发展形势第 2 章基于用户协同过滤推荐技术第 2 章基于用户协同过滤推荐技术 2.1 电子商务推荐系统概述 2.2 协同过滤推荐技术第 3 ...
《Python程序设计与算法基础教程（第二版）》江红余青松，第十章课后习题答案
推荐阅读 <Python程序设计与算法基础教程(第二版)>江红余青松全部章节的课后习题,上机实践,课后答案,案例研究文章目录上机实践:2~4 案例研究:基于模块的库存管理系统上机 ...

AHRS算法代码：磁力计+加计+陀螺版

AHRS算法代码：磁力计+加计+陀螺版相关推荐

最新文章

热门文章