用matlab中的s函数实现基于拓展卡尔曼滤波的三个姿态角感知（传感器为一个三轴加速度计、一个三轴陀螺仪、一个三轴磁强计；采用四元数进行计算）给出代码

这里提供一个基于拓展卡尔曼滤波的姿态解算的 MATLAB 代码，使用的传感器为加速度计、陀螺仪和磁强计，姿态解算采用四元数：

function [roll, pitch, yaw] = EKF_quat(acc, gyro, mag, dt)

    Q = eye(4); % 过程噪声协方差矩阵
    R = diag([0.5 0.5 0.5]); % 观测噪声协方差矩阵

    % 初始化状态向量和协方差矩阵
    x = [1 0 0 0]';
    P = eye(4);

    % 计算加速度、陀螺仪和磁强计的测量值
    acc_norm = norm(acc);
    if acc_norm == 0
        acc_norm = 1e-6;
    end
    acc = acc / acc_norm;

    mag_norm = norm(mag);
    if mag_norm == 0
        mag_norm = 1e-6;
    end
    mag = mag / mag_norm;

    % 计算测量矩阵
    H = [2*x(2) -2*x(3) 2*x(4);
        -2*x(1) 2*x(4) 2*x(3);
        2*x(1) 2*x(2) 2*x(3);
        -2*x(4) -2*x(1) 2*x(2)];
    z = [acc; gyro; mag];
    y = z - H*x;

    % 更新状态向量和协方差矩阵
    F = [0 -gyro(1) -gyro(2) -gyro(3);
        gyro(1) 0 gyro(3) -gyro(2);
        gyro(2) -gyro(3) 0 gyro(1);
        gyro(3) gyro(2) -gyro(1) 0];
    A = eye(4) + 0.5*F*dt;
    x = A*x;
    P = A*P*A' + Q*dt;

    K = P*H'/(H*P*H' + R);
    x = x + K*y;
    P = (eye(4) - K*H)*P;

    % 计算欧拉角
    roll = atan2(2*x(2)*x(3) + 2*x(1)*x(4), x(1)^2 + x(2)^2 - x(3)^2 - x(4)^2);
    pitch = -asin(2*x(2)*x(4) - 2*x(1)*x(3));
    yaw = atan2(2*x(3)*x(4) + 2*x(1)*x(2), x(1)^2 - x(2)^2 - x(3)^2 + x(4)^2);

end

其中，`acc`、`gyro` 和 `mag` 分别表示加速度计、陀螺仪和磁强计的测量值，`dt` 表示采样时间。函数的输出为欧拉角 `roll`、`pitch` 和 `yaw`。

需要注意的是，该代码仅作为参考，实际使用时需要根据实际情况进行调整和优化。