一、介绍

EKF拓展卡尔曼滤波器是px4开源飞控框架采用的核心状态估计方法，EKF2是px4飞控中的对应的软件模块，可以支持各类传感器信号，包括IMU，磁感计，激光测距仪，气压计，激光定位和视觉定位的数据等。
本文以Kerloud飞控支持的px4软件版本为例，系统地介绍下EKF的程序流程，Kerloud飞控是云讷科技的核心产品之一。
Kerloud飞控支持维护的px4软件库位于：https://github.com/cloudkernel-tech/Firmware
Kerloud飞控的详细说明位于：http://cloudkernel-tech.gitee.io/kerloud_mini/

二、工作原理

EKF是经典卡尔曼滤波器的非线性版本，通过对非线性系统进行线性化实现，它是非线性状态估计，导航和GPS设备中的标杆性算法。

三、程序框架

1. 基本程序框架

2. 视觉SLAM融合说明

这里以视觉SLAM的位置融合说明下EKF的融合细节，
首先，假设从机载端电脑或者其他信息可以得到视觉SLAM输出的位姿，可以通过mavlink 协议的VISION_POSITION_ESTIMATE或者ODOMETRY消息输入给飞控，区别在于VISION_POSITION_ESTIMATE只包含位置信息，而ODOMETRY还可以得到速度、姿态和姿态角速度等。

飞控中对应的代码处理在
https://github.com/cloudkernel-tech/Firmware/blob/master_kerloud/src/modules/mavlink/mavlink_receiver.cpp

对应的函数为handle_message_vision_position_estimate()和handle_message_odometry()，我们需要注意的是kerloud飞控默认支持的坐标系为NED坐标系，这点很重要。得到的SLAM信息会通过uorb topic vehicle_visual_odometry发布出来。

EKF2模块(src/modules/ekf2/ekf2_main.cpp)在line 1127开始会接受vehicle_visual_odometry topic的值，给对应位置、速度或者姿态赋值。

External vision innovation 计算：
水平方向计算在: src/lib/ecl/EKF/control.cpp的line 307
竖直方向计算在: vel_pos_fusion.cpp的line 150
Innovation variance计算在vel_pos_fusion.cpp的line 167，对应K值修正计算在line 234行，
最后的状态修正在line 315行。

在使用视觉SLAM融合时，我们需要注意底层飞控的融合设置，包括方差估计，高度，磁感计等，好的效果都需要充分的测试，同时飞行器平台的振动要求也需要考虑在内。

四、更多信息

• 云讷科技官方主页：https://cloudkernel-tech.github.io/
• EKF wiki：https://en.wikipedia.org/wiki/Extended_Kalman_filter
• EKF tuning guide: https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html
• Px4 vision position estimation:
  https://docs.px4.io/master/en/ros/external_position_estimation.html

云讷科技公众号二维码：