四元数姿态解算c语言例程_STM32/STM8

使用四元数进行姿态解算

先定义Kp，Ki，以及halfT。

Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度。

halfT，姿态解算时间的一半。此处解算姿态的速度是200Hz，因此halfT为0.0025

#define Kp 2.0f

#define Ki 0.002f

#define halfT 0.0025f

初始化四元数

float q0=1,q1=0,q2=0,q3=0;

定义姿态解算误差的积分

float exInt=0,eyInt=0,ezInt=0;

以下为姿态解算函数

参数gx，gy，gz分别对应三个轴的角速度，单位为弧度/秒。

参数ax，ay，az分别对应三个轴的加速度原始数据。

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

float norm;

float vx, vy, vz;

float ex, ey, ez;

将加速度的原始数据，归一化，得到单位加速度

Norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;

ay = ay / norm;

az = az / norm;

把四元数换算成“方向余弦矩阵”中第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，