• 大小: 4KB
    文件类型: .c
    金币: 2
    下载: 1 次
    发布日期: 2021-06-12
  • 语言: 其他
  • 标签:

资源简介

九轴核心融合算法原代码,是一份很不错的参考代码,可以直接用。

资源截图

代码片段和文件信息

//=====================================================================================================
// 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 

评论

共有 条评论

相关资源