資源簡介
九軸核心融合算法原代碼,是一份很不錯的參考代碼,可以直接用。
代碼片段和文件信息
//=====================================================================================================
//?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?
評論
共有 條評論