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

以下是基于拓展卡尔曼滤波的三个姿态角感知的MATLAB代码：

function [roll, pitch, yaw] = ekf_attitude_estimation(acc, gyro, mag, Ts)
% acc, gyro, mag: 3-axis accelerometer, gyroscope, and magnetometer measurements
% Ts: sampling time
% roll, pitch, yaw: estimated Euler angles

% Define initial states
x = [0;0;0;1;0;0;0]; % [phi; theta; psi; q0; q1; q2; q3]
P = eye(7);

% Define process noise covariance
Q = diag([0.1; 0.1; 0.1; 0.01; 0.01; 0.01; 0.01]);

% Define measurement noise covariance
R = diag([0.1; 0.1; 0.1]);

% Define measurement matrix
H = [eye(3), zeros(3,4)];

% Define identity matrix
I = eye(7);

% Define gyroscope bias
bias = zeros(3,1);

% Define magnetic declination angle
declination = 0; % in radians

% Initialize estimated Euler angles
roll = zeros(size(acc,2),1);
pitch = zeros(size(acc,2),1);
yaw = zeros(size(acc,2),1);

for i = 1:size(acc,2)
    % Predict state and covariance using quaternion kinematics
    omega = deg2rad(gyro(:,i)) - bias;
    omega_norm = norm(omega);
    if omega_norm == 0
        q_dot = zeros(4,1);
    else
        q_dot = 0.5*[0; omega]/omega_norm * [x(4); x(5); x(6); x(7)];
    end
    F = [eye(3), -Ts*skew(omega);
         zeros(4,3), eye(4)];
    x_minus = F * x;
    P_minus = F * P * F' + Q;

    % Update state and covariance using measurement
    z = [acc(:,i); mag(:,i)] / norm([acc(:,i); mag(:,i)]);
    y = z - H * x_minus;
    S = H * P_minus * H' + R;
    K = P_minus * H' / S;
    x = x_minus + K * y;
    P = (I - K * H) * P_minus;

    % Compute Euler angles from quaternion
    roll(i) = atan2(2*(x(4)*x(5)+x(6)*x(7)), 1-2*(x(5)^2+x(6)^2));
    pitch(i) = asin(2*(x(4)*x(6)-x(5)*x(7)));
    yaw(i) = atan2(2*(x(4)*x(7)+x(5)*x(6)), 1-2*(x(6)^2+x(7)^2)) + declination;
end

end

function S = skew(v)
% Skew-symmetric matrix
S = [0 -v(3) v(2);
     v(3) 0 -v(1);
     -v(2) v(1) 0];
end

其中，`deg2rad`函数将角度转换为弧度，`skew`函数计算向量的反对称矩阵。在主函数 `ekf_attitude_estimation` 中，使用四元数进行姿态估计，并通过拓展卡尔曼滤波进行状态估计和协方差更新。最终，通过计算四元数到欧拉角的转换，得到三个姿态角。