hot wire! hot wire! Px4-ecl replaces matlab with Python

PX4-ECL Perhaps the best of its kind （ Or one of them ）, Many developers engaged in UAV filter development or similar multi-sensor fusion have read the code of this project .github Address

But this project has always had a regret , The matrix solution part of Kalman filter uses matlab Accomplished , Here's the picture . What's the problem with this ？

By the way , You have to pass PX4-ECL\EKF\matlab\scripts\Inertial Nav EKF\GenerateNavFilterEquations.m To model and generate c++ matrix .

1. matlab Is the charge , Closed source , And px4 The open source , It seems a little out of place
2. Influence communication , This is all. matlab Common weaknesses of open source projects , Share it , Then others must have matlab, But matlab Is the charge , And it's expensive
3. matlab yes MathWorks Products of the company , Maybe one day I give up ？
4. Of course there is a solution , use px4 Code for , Unchanged model , Or manually derive , Write code manually （ That's ok ~）
   
   But , finally ,matlab this 11.9% Code for , Can become 0 了 . because python coming ,python It's free. , And it's open source . Have to admit , Now almost all fields ,python Can replace matlab. Of course , except simulink Visual modeling of , And some of the toolbox. except , These two can replace , If you find , In your app ,matlab Can't be python Replace , Then we can talk ~

What about? ？ Are you ready to try ？ It is a pity that , Direct operation python Procedure words , The following errors may be encountered .Sympy Doesn't seem to support 21 Matrix and a scalar generate a 31 Matrix , But that's okay , You can replace the scalar with 1*1 The matrix is OK .

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

Modify the code as follows ,

 # attitude update equation ( The modification is here )
psiNew = Matrix([psi + daz])
# velocity update equations
velNew = Matrix([vn,ve]) + Tbn*Matrix([dvx,dvy])
# Define the state vectors
stateVector = Matrix([vn,ve,psi])

Run it again , We found that the program was running successfully , Code files are also generated ,

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