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

以下是一个基于拓展卡尔曼滤波的三个姿态角感知的 MATLAB 代码，使用了一个三轴加速度计、一个三轴陀螺仪和一个三轴磁强计进行传感器数据采集，并且使用四元数进行计算：

function [Roll, Pitch, Yaw] = extendedKalmanFilter(acc, gyro, mag, Ts, q, r)
% acc: 三轴加速度计数据，3xN数组，N是采样点数
% gyro: 三轴陀螺仪数据，3xN数组，N是采样点数
% mag: 三轴磁强计数据，3xN数组，N是采样点数
% Ts: 采样时间间隔（秒）
% q: 系统噪声协方差矩阵
% r: 测量噪声协方差矩阵

N = size(acc,2); % 采样点数

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

% 初始化输出变量
Roll = zeros(1,N);
Pitch = zeros(1,N);
Yaw = zeros(1,N);

for i=1:N
    % 读取传感器数据
    a = acc(:,i);
    g = gyro(:,i);
    m = mag(:,i);
    
    % 计算采样时间间隔
    if i > 1
        dt = Ts*(i-1) - Ts*(i-2);
    else
        dt = Ts;
    end
    
    % 计算状态转移矩阵
    F = [eye(4)+0.5*dt*skew(g)];
    
    % 计算控制输入矩阵
    B = [zeros(4,3); -0.5*dt*eye(3)];
    
    % 预测状态和协方差
    x_minus = F*x;
    P_minus = F*P*F' + B*q*B';
    
    % 计算测量矩阵
    H = [-2*x(3) 2*x(4) -2*x(1) 2*x(2);
         2*x(2) 2*x(1) 2*x(4) 2*x(3);
         2*x(1) -2*x(2) -2*x(3) 2*x(4)];
    
    % 计算卡尔曼增益
    K = P_minus*H'/(H*P_minus*H' + r);
    
    % 计算测量向量
    z = [a; m];
    
    % 更新状态和协方差
    x = x_minus + K*(z - H*x_minus);
    P = (eye(4) - K*H)*P_minus;
    
    % 将四元数转换为欧拉角
    [Roll(i), Pitch(i), Yaw(i)] = quat2angle(x');
end
end

function S = skew(v)
% 将向量转换为反对称矩阵
S = [    0   -v(3)  v(2);
      v(3)     0  -v(1);
     -v(2)  v(1)     0];
end

其中，`acc`、`gyro` 和 `mag` 分别是三轴加速度计、三轴陀螺仪和三轴磁强计的数据，每个传感器的数据都是一个 $3 \times N$ 的矩阵，其中 $N$ 是采样点数。`Ts` 是采样时间间隔，`q` 是系统噪声协方差矩阵，`r` 是测量噪声协方差矩阵。`Roll`、`Pitch` 和 `Yaw` 分别是三个欧拉角的输出结果。

该函数的主要步骤如下：

1. 初始化状态向量和协方差矩阵。
2. 对于每个采样点，计算状态转移矩阵、控制输入矩阵和测量矩阵。
3. 预测状态和协方差。
4. 计算卡尔曼增益。
5. 计算测量向量。
6. 更新状态和协方差。
7. 将四元数转换为欧拉角。

在代码中，我们使用了一个 `skew` 函数，将向量转换为反对称矩阵。`quat2angle` 函数将四元数转换为欧拉角。