资源简介
九轴核心融合算法原代码,是一份很不错的参考代码,可以直接用。
代码片段和文件信息
//=====================================================================================================
// AHRS.c
// S.O.H. Madgwick
// 25th August 2010
//=================================================================================================// Description:
//
// 四元的“DCM的过滤器”的实施[Mayhony等]。采用了磁性失真
// 我的过滤器[Madgwick]省去了补偿算法
引用
// 通量(BX BZ)的方向,是预定义的和限制的磁场效应
扭曲到偏航
// 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
// 方向. 见我概述使用这个应用程序中的四元报告。
//
// 用户必须调用 ‘AHRSupdate()‘ 校准每个采样周期和解析 gyroscope (‘gx‘ ‘gy‘ ‘gz‘)
// 加速度计 (‘ax‘ ‘ay‘ ‘ay‘) and 磁力仪(‘mx‘ ‘my‘ ‘mz‘) data. Gyroscope units are
// 弧度/秒,加速度计和磁强计单位归为载体无关,是归为载体。
//
//=====================================================================================================
//----------------------------------------------------------------------------------------------------
// Header files
#include “AHRS.h“
#include
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f // 比例增益支配收敛率accelerometer/magnetometer
#define Ki 0.005f // 积分增益执政速率陀螺仪的衔接gyroscopeases
#define halfT 0.5f // 采样周期的一半
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1 q1 = 0 q2 = 0 q3 = 0; // 四元数的元素,代表估计方向
float exInt = 0 eyInt = 0 ezInt = 0; // 按比例缩小积分误差
//====================================================================================================
// 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
评论
共有 条评论