好消息！好消息！ PX4-ECL 以python替代matlab

PX4-ECL或許是同類項目中最優秀的（或者之一吧），從事無人機濾波開發或者類似多傳感器融合的開發者很多都讀過這個項目的代碼。github地址

但這個項目一直以來有個遺憾，卡爾曼濾波的矩陣求解部分是用matlab完成的，如下圖。這麼做有什麼問題呢？

順便說一下，之前要通過PX4-ECL\EKF\matlab\scripts\Inertial Nav EKF\GenerateNavFilterEquations.m 來建模和生成c++矩陣。

1. matlab是收費的，閉源的，與px4的開源，顯得有點兒格格不入
2. 影響傳播，這是所有matlab開源項目的共同弱點，分享出去，那麼別人得有matlab，可是matlab是收費的，而且很貴
3. matlab是MathWorks公司的產品，說不定哪天放棄了呢？
4. 當然有個解決辦法，用px4的代碼，不改模型，或者手動推導，手動寫代碼（這也是可以的~）
   
   可是，終於，matlab這11.9%的代碼，可以變成0了。因為python來了，python是免費的，而且是開源的。不得不承認，現在幾乎所有領域，python都可以替代matlab。當然，除了simulink的可視化建模，和一些toolbox。除了，這兩個都可以替代，如果您發現，您的應用中，matlab不可以被python替換，那咱們可以聊聊~

怎麼樣？是不是躍躍欲試了？很可惜，直接運行python程序的話，可能會遇到以下錯誤。Sympy似乎不支持21的矩陣和一個標量生成一個31的矩陣，不過沒關系，可以把標量換成1*1的矩陣就可以了。

Generating yaw estimator code ...
Traceback (most recent call last):
File "main.py", line 629, in <module>
generate_code()
File "main.py", line 624, in generate_code
yaw_estimator()
File "main.py", line 430, in yaw_estimator
newStateVector = Matrix([velNew,psiNew])
File "E:\1-bin\anaconda\lib\site-packages\sympy\matrices\dense.py", line 420, in __new__
return cls._new(*args, **kwargs)
File "E:\1-bin\anaconda\lib\site-packages\sympy\matrices\dense.py", line 432, in _new
rows, cols, flat_list = cls._handle_creation_inputs(*args, **kwargs)
File "E:\1-bin\anaconda\lib\site-packages\sympy\matrices\matrices.py", line 2137, in _handle_creation_inputs
flat_list.append(cls._sympify(in_mat[j][i]))
TypeError: 'Add' object is not subscriptable

修改代碼為如下所示，

 # attitude update equation (修改的是這裡)
psiNew = Matrix([psi + daz])
# velocity update equations
velNew = Matrix([vn,ve]) + Tbn*Matrix([dvx,dvy])
# Define the state vectors
stateVector = Matrix([vn,ve,psi])

再運行一下，我們發現程序運行成功，代碼文件也生成了，

F:\1-own\ecl\ecl\PX4-ECL\EKF\python\ekf_derivation>py main.py
Starting code generation:
Creating symbolic variables ...
Defining state propagation ...
Computing state propagation jacobian ...
Computing covariance propagation ...
Simplifying covariance propagation ...
Writing covariance propagation to file ...
Generating heading observation code ...
Generating gps heading observation code ...
Generating mag observation code ...
Generating declination observation code ...
Generating airspeed observation code ...
Generating sideslip observation code ...
Generating optical flow observation code ...
Generating body frame velocity observation code ...
Generating body frame acceleration observation code ...
Generating yaw estimator code ...
Code generation finished!

2020/12/23 周三 21:12 43,186 3Dmag_fusion_generated_compare.cpp
2020/12/23 周三 21:35 15,257 3Dmag_generated.cpp
2020/12/23 周三 21:35 14,441 3Dmag_generated_alt.cpp
2020/12/23 周三 21:34 2,840 3Dmag_innov_var_generated.cpp
2020/12/23 周三 21:12 43,133 acc_bf_fusion_generated_compare.cpp
2020/12/23 周三 21:37 11,017 acc_bf_generated.cpp
2020/12/23 周三 21:37 11,001 acc_bf_generated_alt.cpp
2020/12/23 周三 21:12 25,708 beta_fusion_generated_compare.cpp
2020/12/23 周三 21:36 6,251 beta_generated.cpp
2020/12/23 周三 21:34 32,739 covariance_generated.cpp
2020/12/23 周三 21:12 92,970 covariance_generated_compare.cpp
2020/12/23 周三 21:12 29,263 flow_fusion_generated_compare.cpp
2020/12/23 周三 21:36 11,359 flow_generated.cpp
2020/12/23 周三 21:37 10,984 flow_generated_alt.cpp
2020/12/23 周三 21:12 7,997 gps_yaw_fusion_generated_compare.cpp
2020/12/23 周三 21:34 3,821 gps_yaw_generated.cpp
2020/12/23 周三 21:12 7,675 mag_decl_fusion_generated_compare.cpp
2020/12/23 周三 21:35 2,077 mag_decl_generated.cpp
2020/12/23 周三 21:12 11,120 tas_fusion_generated_compare.cpp
2020/12/23 周三 21:35 3,348 tas_generated.cpp
2020/12/23 周三 21:12 422 util.h
2020/12/23 周三 21:37 12,612 vel_bf_generated.cpp
2020/12/23 周三 21:37 13,346 vel_bf_generated_alt.cpp
2020/12/23 周三 21:37 742 yaw_estimator_covariance_prediction_generated.cpp
2020/12/23 周三 21:38 2,017 yaw_estimator_measurement_update_generated.cpp
2020/12/23 周三 21:12 11,173 yaw_fusion_generated_compare.cpp
2020/12/23 周三 21:34 3,972 yaw_generated.cpp