用matlab中的s函数，实现基于四元数的拓展卡尔曼滤波，计算无人机的三轴姿态角（传感器为三轴加速度计，三轴陀螺仪，三轴磁强计）

拓展卡尔曼滤波（EKF）是一种常用于估计系统状态的算法，它通过将传感器测量值与系统模型结合起来，估计系统的状态。在无人机姿态角估计中，可以使用加速度计、陀螺仪和磁强计三种传感器来测量飞机的运动状态。其中加速度计可以测量飞机的重力加速度分量，陀螺仪可以测量飞机的角速度，磁强计可以测量飞机在磁场中的方向。

在使用EKF算法进行姿态角估计时，需要建立系统的状态方程和观测方程。系统的状态方程可以用欧拉角或四元数来表示，观测方程则是传感器测量值与系统状态之间的关系。本例中使用四元数来表示系统的状态，因为它具有很好的数学性质，可以避免欧拉角存在的万向锁问题。

具体步骤如下：

1. 初始化系统状态和协方差矩阵

在开始运行EKF算法之前，需要初始化系统状态和协方差矩阵。状态向量包括四元数、角速度和偏差等状态变量，协方差矩阵是一个对角矩阵，表示各个状态变量的不确定性。

2. 根据系统动力学模型，预测系统状态

使用系统的状态方程和控制输入（在无人机姿态角估计中，控制输入为姿态角速度），预测系统的状态。这一步可以使用matlab中s函数来实现。

3. 计算预测状态的协方差矩阵

使用预测状态的协方差矩阵和系统噪声协方差矩阵，计算预测状态的协方差矩阵。在本例中，系统噪声包括姿态角速度的随机误差和偏差。

4. 根据观测方程，计算卡尔曼增益

使用观测方程和预测状态，计算卡尔曼增益。卡尔曼增益表示预测状态和实际状态之间的差距，是用于融合传感器测量值和系统模型的关键。

5. 根据传感器测量值，更新预测状态

使用卡尔曼增益和传感器测量值，更新预测状态。在本例中，传感器测量值包括加速度计、陀螺仪和磁强计的测量值。

6. 计算更新后状态的协方差矩阵

使用更新后的状态和协方差矩阵，计算更新后状态的协方差矩阵。

7. 重复步骤2-6，直到结束

重复以上步骤，直到算法收敛或达到最大迭代次数。

总之，使用matlab中的s函数实现基于四元数的拓展卡尔曼滤波可以实现无人机的三轴姿态角估计。