使用PX4的ECL进行多传感器数据融合的后处理

写在前边

• ecl是开源无人机项目PX4使用的算法库，使用ekf（扩展卡尔曼滤波）进行imu等多种传感器的数据融合
• 然而ecl不提供数据后处理功能
• 能使用ecl进行多传感器数据融合的后处理是很有必要的，这样便于参数调试，和二次开发的调试，从而缩短开发周期，降低开发成本
• 鉴于以上几点作者fork了ecl的项目，并添加了后处理功能
• 在添加后处理功能时的基本原则是，尽量做到不更改ecl的任何代码，实际上却是没有更改任何代码文件
• 实际上整个更改只是在CMakeLists.txt中添加了下边这一行

add_subdirectory(airdata)
add_subdirectory(EKF) # 
add_subdirectory(geo)
add_subdirectory(geo_lookup)
add_subdirectory(main) # 这一行是新添加的

• 可以看出一个叫做main的文件夹被添加到了和ekf以及test等的同级目录，这个文件夹中包含了生成后处理程序所需要的CMakeLists.txt和所有代码文件，而ekf则使用add_subdirectory(EKF) #这一行生成的动态库
• 点击此处前往本项目的github仓库

入门

按照以下步骤可完成整个项目的克隆，生成，编译和运行。

1. clone 项目到本地仓库

git clone https://github.com/akstuki/PX4-ECL.git

2. （这一步不是必须的）建议新建目录build，这样会比较干净

mkdir build/
cd build/

3. 生成项目（生成makefile文件）

$ cmake ..
-- The CXX compiler identification is GNU 7.4.0
-- Check for working CXX compiler: /usr/bin/c++.exe
-- Check for working CXX compiler: /usr/bin/c++.exe -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- build type is RelWithDebInfo
-- PX4 ECL: Very lightweight Estimation & Control Library v1.9.0-rc1-496-g5ca644e
-- Configuring done
-- Generating done
-- Build files have been written to: /cygdrive/f/1-own/0-code/0alforithmCode/px4/ecl/PX4-ECL/build

4. 编译生成可运行文件

$ make
Scanning dependencies of target matrix
[  2%] Completed 'matrix'
[ 21%] Built target matrix
Scanning dependencies of target prebuild_targets
[ 21%] Built target prebuild_targets
[ 26%] Built target ecl_airdata
[ 31%] Built target ecl_geo_lookup
[ 36%] Built target ecl_geo
[ 89%] Built target ecl_EKF
Scanning dependencies of target postEcl
[ 92%] Building CXX object main/CMakeFiles/postEcl.dir/main.cpp.o
[ 94%] Building CXX object main/CMakeFiles/postEcl.dir/csvparser.cpp.o
[ 97%] Building CXX object main/CMakeFiles/postEcl.dir/PostEkf.cpp.o
[100%] Linking CXX executable postEcl.exe
[100%] Built target postEcl

5. 运行程序（程序仅仅需要一个参数，就是px4的日志文件）

$ ./postEcl.exe 09_26_14_sensor_combined_0.csv
start ecl main...
47634158: reset position to last known position
47634158: reset velocity to zero
51113397: mag yaw fusion numerical error - covariance reset
51534173: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9)
57718962: mag yaw fusion numerical error - covariance reset
baro hgt timeout - reset to baro
67032581: mag yaw fusion numerical error - covariance reset
68114987: reset position to last known position
68114987: reset velocity to zero
68114987: stopping navigation
baro hgt timeout - reset to baro
baro hgt timeout - reset to baro
84041372: reset position to last known position
84041372: reset velocity to zero
84041372: stopping navigation
finished.

6. 可以看到程序运行结束后生成一个输出文件ecloutput.csv，包含ekf的所有状态

$ ls -l
总用量 6848
-rwxrwxr-x+ 1 xiaoqiang xiaoqiang 1492459 9月  29 2018 09_26_14_sensor_combined_0.csv
-rwxrw-r--+ 1 xiaoqiang xiaoqiang    1025 1月   4 22:37 cmake_install.cmake
drwxrwxr-x+ 1 xiaoqiang xiaoqiang       0 1月   5 21:51 CMakeFiles
-rwxrw-r--+ 1 xiaoqiang xiaoqiang 2612997 1月   5 22:08 ecloutput.csv
-rwxrw-r--+ 1 xiaoqiang xiaoqiang    7687 1月   5 21:50 Makefile
-rwxrwxr-x+ 1 xiaoqiang xiaoqiang 2889190 1月   5 21:52 postEcl.exe

下一步工作

• 添加更多数据格式的支持框架
• 添加参数配置接口 （或许以配置文件方式）
• 另外，可以将CMakeList.txt放到外层，把ecl以external的方式导入（本项目暂时不考虑这种方式）