ardupilot开发 --- EKF 篇

1. EKF1~EKF3

待续…

2. Ardupilot滤波算法的演变

使用DCM还是EKF?
AHRS_EKF_USE: set to “1” to use the EKF, “0” to use DCM for attitude control and inertial nav (Copter-3.2.1) or ahrs dead reckoning (Plane) for position control. In Copter-3.3 (and higher) this parameter is forced to “1” and cannot be changed.

3. EKF3

• AHRS_EKF_TYPE = 3
  EK2_ENABLE = 0
  EK3_ENABLE = 1
• 飞控有多少个IMU就会运行多少个 EKF core 或称 EKF lane
  EK3_IMU_MASK 参数决定哪些IMU用于运行EKF lane.
• 只有一个EKF lane 被用作飞机的状态估计AHRS，由参数EK3_PRIMARY决定，0 selects the first IMU lane in the EK3_IMU_MASK, 1 the second。这个用作AHRS的EKF3 lane 被称为 primary lane
• 当 primary lane 出问题（误差很大，innovation大）时就会发生lane切换，即将其他表现正常的 EKF lane 切换为 primary lane，称为 Lane Switching
• Lane Switching 由传感器的亲和度 EK3_AFFINITY 和错误阈值 EK3_ERR_THRESH 决定。
• EK3_AFFINITY 和 EK3_ERR_THRESH 如何设置？
  

3.1 例子

某飞控具有：
3 IMUs
1 Barometer
2 GPS
2 Airspeeds
3 Magnetometers
EKF Lane 的传感器分配情况如下：

原文：Conventionally, each lane uses the primary instance of the Airspeed, Barometer, GPS and Magnetometer sensors.
疑问：3个IMU（Lane）2个GPS，怎么确定哪个Lane使用的是哪个GPS呢？？Lane3使用的为什么是GPS1而不是2呢？？
去源代码中一探究竟，看看这些传感器是如何被分配给 EKF lanes 的！！
待续…

4. 代码解析

在类中找到

private:
    uint8_t num_cores; // number of allocated cores
    uint8_t primary;   // current primary core
    NavEKF3_core *core = nullptr;
1
2
3
4

可知 core 肯定是指向了 NavEKF3_core 数组的首地址。
在类的.cpp中寻找 num_cores （因为core和num_cores肯定是关联出现的，找到num_cores就可以找到cores）：

// Call constructors on all cores
for (uint8_t i = 0; i < num_cores; i++) {
	new (&core[i]) NavEKF3_core(this);
}
1
2
3
4

可知 core 就是指向 NavEKF3_core 数组的首地址。
想知道传感器分配情况，就得进入构造函数NavEKF3_core(this)中寻找答案：
待续…