Madgwick AHRS MATLAB 程序

以下是一个简单的Madgwick AHRS MATLAB程序，用于计算四元数和欧拉角：

function [quat, euler] = madgwickAHRS(acc, gyr, mag, sampleFreq, beta)
% Madgwick AHRS算法
% 输入：
% acc：3x1加速度计读数（m/s^2）
% gyr：3x1陀螺仪读数（rad/s）
% mag：3x1磁力计读数（uT）
% sampleFreq：采样频率（Hz）
% beta：算法收敛率（默认为0.1）
% 输出：
% quat：4x1四元数
% euler：3x1欧拉角（rad）

% 初始化四元数
quat = [1 0 0 0]';

% 重力向量
g = [0 0 1]';

% 计算测量值的单位向量
acc = acc / norm(acc);
mag = mag / norm(mag);

% 计算测量值的参考向量
hx = 2 * mag(1) * (0.5 - quat(3)^2 - quat(4)^2) + 2 * mag(2) * (quat(2)*quat(3) - quat(1)*quat(4)) + 2 * mag(3) * (quat(2)*quat(4) + quat(1)*quat(3));
hy = 2 * mag(1) * (quat(2)*quat(3) + quat(1)*quat(4)) + 2 * mag(2) * (0.5 - quat(2)^2 - quat(4)^2) + 2 * mag(3) * (quat(3)*quat(4) - quat(1)*quat(2));
hz = 2 * mag(1) * (quat(2)*quat(4) - quat(1)*quat(3)) + 2 * mag(2) * (quat(3)*quat(4) + quat(1)*quat(2)) + 2 * mag(3) * (0.5 - quat(2)^2 - quat(3)^2);
b = [sqrt(hx^2 + hy^2) 0 hz]';

% 计算估计重力向量
a = quatrotate(quatinv(quat), g)';

% 计算估计磁场向量
m = quatrotate(quatinv(quat), b)';

% 计算误差向量
err = cross(a, acc) + cross(m, mag);

% 计算梯度下降方向
J = [0 -2*quat(3) 2*quat(2);
     2*quat(3) 0 -2*quat(1);
     -2*quat(2) 2*quat(1) 0];
step = J * err;

% 更新四元数
qDot = 0.5 * quatmultiply(quat', [0 gyr'])';
quat = quat + qDot * (1 / sampleFreq - beta);

% 归一化四元数
quat = quat / norm(quat);

% 计算欧拉角
euler = quat2eul(quat', 'ZYX')';

end

使用方法：

1. 将上述代码复制到MATLAB编辑器中，并保存为madgwickAHRS.m文件；
2. 在MATLAB命令行中输入以下指令，初始化参数：

acc = [0 0 -9.81]'; % 初始化加速度计读数
gyr = [0 0 0]'; % 初始化陀螺仪读数
mag = [0 0 0]'; % 初始化磁力计读数
sampleFreq = 100; % 采样频率（Hz）
beta = 0.1; % 算法收敛率

3. 在MATLAB命令行中输入以下指令，运行程序：

[quat, euler] = madgwickAHRS(acc, gyr, mag, sampleFreq, beta);

4. 程序输出四元数和欧拉角。