姿态控制的ekf预测控制与结算学习

代码框架篇

代码的主要结构由ekf2_main.cpp, estimator_interface.cpp, 和ekf.cpp，ekf_helper.cpp 互相交互，在加上底层的的一系列处理文件。
事实上，之前花费了一周的时间，画的流程图意义和价值很大，这让我在程序代码分析的时候更加直观
流程图的连接见：https://download.csdn.net/download/weixin_39350416/11546415
接下来我该做什么呢，主要分析一下，updated（）函数中的五个步骤

1. 预测状态
2. 预测协方差
3. 控制融合模式
4. 运行地形估计
5. 计算输出状态（输出状态的论文已经梳理过了，但是程序还需要再仔细琢磨）

然后我们现在从预测状态开始
predictState();
predictCovariance();
这两个函数的主要任务就是执行主过滤器的状态和协方差预测；

controlFusionModes();
这个函数控制融合观测数据

刚刚把第一个状态预测函数过了下，这个函数在ekf.cpp中
确实是根据δ角度和δ速度，校正当前的角度和速度，然后校正垂直方向的速度，和位置信息。

现在过预测协方差，这个函数在协方差的大文件里，covariance.cpp中
这个预测协方差主要做协方差的计算，但是这个内部协方差是计算还是置位，都是有条件的，比如加速度计在抖震比较严重的时候，或者测到垂直方向频繁刷新的时候，融合控制会判断一些情况，来处理协方差是否要进入计算。
主要的几个逻辑有：

计算静止过程的噪声方差
将参数指定的速率陀螺仪偏差（rad / s ** 2）的变化率转换为自上次更新以来的delta角度（rad）的预期变化
将参数指定的加速度计偏差（m / s ** 3）的变化率转换为自上次更新以来delta速度的预期变化（m / s）
抑制机动水平的imu accel偏差的学习过高以防止传感器非线性的影响或检测到不良的加速度数据

1. 角加速度计偏差＞学得的陀螺仪偏差限制 或者 加速度计偏差 ＞ 学得的加速度计偏差限制 或者 垂直加速恶劣检测有效 然后判断
   加速偏差抑制是否有效
2. 如果地球坐标系的 变量变化太大，或者我们未进行三轴融合，，则不进行地球坐标系变量的增加（即，不进入计算）
3. 如果机身坐标系的 变量变化太大，或者我们未进行三轴融合，，则不进行机身坐标系变量的增加（即，不进入计算）
4. 如果风速状态变量变化太大，或者我们根本没有使用风速估计，则不进行风速状态变量的增加（即，不进入计算）
5. 使用固定过程模型为这些状态构建过程噪声方差对角线，这些是运动状态，它们的误差增长由IMU噪声方差分别控制，构建过程中的变量都来自于前面的处理过的变量
6. 分配噪声变量，系统的输入是3个delta角和3个delta速度
7. 如果发现垂直加速的检测糟糕，则放大噪声
8. 预测协方差，计算四元数，速度，位置和陀螺仪偏置状态的方差和上对角协方差
9. 添加非IMU的过程噪音
10. 如果IMU delta速度偏差估计被禁止，则不计算这些协方差项
11. 除非我们使用3轴融合，否则不要对磁场状态进行协方差预测
12. 除非我们使用它们，否则不要对风状态进行协方差预测
13. 如果我们的总位置变化达到100米，则停止位置协方差增长，如果我们一段时间内失去gps信号，就会发生这种情况
14. 协方差矩阵是对称的，所以将上半部分复制到下半部分，然后复制对角线变量
15. 修复协方差矩阵中的粗差，并确保未使用状态的行和列为零（这是个单独的函数）叫fixCovarianceErrors();

我详细看了下修正协方差误差的函数，主要还是判断，一系列处理标志位
例如变量变化范围过大，对变量做限制或者其他处理
这个函数作为预测故障的最后一步，不能全部依赖他，最好在前面能处理了就处理掉。