fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 07:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Convert magnetic field observation methods to use SymPy generated code (#879)

Browse Source 
* EKF: Add comparison test for mag field fusion generated code

* EKF: convert mag field fusion to use SymPy generated code

* EKF: Add test comparison program for yaw fusion equations

* Stop setting 0 to 0

* Reduce if/else statement to only if

* EKF: more accurate implementation for sequential fusion of magnetometer data

* test: update change indicator

* Use matrix::SparseVector<float, 24, ...> for observation jacobian

* Adapt the auto code generation to allow for different bracket styles

* Add auto generated code for mag fusion

* Add generic computation of KHP

* Apply generic computation of KHP to mag fusion

* tests: update change indicator

* tests: update change indicator

Co-authored-by: kamilritz <kritz@ethz.ch>
This commit is contained in:
Paul Riseborough
2020-08-11 18:57:22 +10:00
committed by GitHub
parent ec93490890
commit 21cc46edd7
12 changed files with 2574 additions and 782 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/python/ekf_derivation/code_gen.py
+5 -11
View File
@@ -27,26 +27,20 @@ class CodeGenerator:
write_string = write_string + "\n\n"
self.file.write(write_string)
def write_matrix(self, matrix, identifier, is_symmetric=False):
def write_matrix(self, matrix, variable_name, is_symmetric=False, pre_bracket="(", post_bracket=")"):
write_string = ""
if matrix.shape[0] * matrix.shape[1] == 1:
write_string = write_string + identifier + " = " + self.get_ccode(matrix[0]) + ";\n"
write_string = write_string + variable_name + " = " + self.get_ccode(matrix[0]) + ";\n"
elif matrix.shape[0] == 1 or matrix.shape[1] == 1:
for i in range(0,len(matrix)):
if (identifier == "Kfusion"):
# Vector f format used by Kfusion
write_string = write_string + identifier + "(" + str(i) + ") = " + self.get_ccode(matrix[i]) + ";\n"
else:
# legacy array format used by Hfusion
write_string = write_string + identifier + "[" + str(i) + "] = " + self.get_ccode(matrix[i]) + ";\n"
write_string = write_string + variable_name + pre_bracket + str(i) + post_bracket + " = " + self.get_ccode(matrix[i]) + ";\n"
else:
for j in range(0, matrix.shape[1]):
for i in range(0, matrix.shape[0]):
if j >= i or not is_symmetric:
write_string = write_string + identifier + "(" + str(i) + "," + str(j) + ") = " + self.get_ccode(matrix[i,j]) + ";\n"
# legacy array format
# write_string = write_string + identifier + "[" + str(i) + "][" + str(j) + "] = " + self.get_ccode(matrix[i,j]) + ";\n"
write_string = write_string + variable_name + pre_bracket + str(i) + "," + str(j) + post_bracket + " = " + self.get_ccode(matrix[i,j]) + ";\n"
write_string = write_string + "\n\n"
self.file.write(write_string)