飞控代码是怎么实现控制的（飞控开发理论基础知识）

飞控代码是怎么实现控制的（飞控开发理论基础知识）a、 状态一步预测（2）其中和分别代表运动方程中和观测中的噪声向量，其k时刻的协方差矩阵可表示为，状态量在k时刻的协方差矩阵可表示为，卡尔曼滤波器的步骤如下（4）观测方程的雅各比矩阵可表示为：2、基于扩展卡尔曼滤波器融合算法（1）考虑系统的运动方程和观测方程中存在的噪声影响，系统方程可表示为：

1、四元数运动学模型

（1）变量选择四元数，状态量的运动方程可表示为：

（2）加速度计数值作为系统的观测量，则系统的观测方程可表示为：

（3）状态方程的雅各比矩阵可表示为：

（4）观测方程的雅各比矩阵可表示为：

2、基于扩展卡尔曼滤波器融合算法

（1）考虑系统的运动方程和观测方程中存在的噪声影响，系统方程可表示为：

（2）其中和分别代表运动方程中和观测中的噪声向量，其k时刻的协方差矩阵可表示为，状态量在k时刻的协方差矩阵可表示为，卡尔曼滤波器的步骤如下

a、 状态一步预测

b、 协方差一步预测

c、 计算卡尔曼增益

d、状态更新

e、协方差更新

（3）在四元数更新过程中，需要注意对四元数进行归一化处理。

（4）协方差矩阵自适应正定。过程和观测协方差矩阵Q R决定着误差修正权值大小，其值越小，则对该信息的信任度越高，两种测量在不同的状态下受信任度不一致，因此需要在更新过程中根据状态信息自适应整定协方差矩阵的大小动态选择信任的传感器。需要注意的是，观测方程按照理想的重力加速度进行计算，而在实际情况中由于运动会引入非重力加速度，导致测量值与理想值不相符，此时应降低对加速度传感器的信任度。因此，根据加速度计的数据与重力加速度数值的偏差调整观测协方差矩阵，当测量加速度近似重力加速度时，降低方差，若测量加速度远离重力加速度，则增大方差。

（5）利用四元数反向求解角速度