mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
77b11129fa
* added python script with ekf derivation (WIP) Signed-off-by: RomanBapst <bapstroman@gmail.com> * worked on c code auto-generation Signed-off-by: RomanBapst <bapstroman@gmail.com> * save before variable name change Signed-off-by: RomanBapst <bapstroman@gmail.com> * changed symbol names Signed-off-by: RomanBapst <bapstroman@gmail.com> * added codegeneration class Signed-off-by: RomanBapst <bapstroman@gmail.com> * improve 3D mag fusion derivation Signed-off-by: RomanBapst <bapstroman@gmail.com> * EKF: Extend ekf sympy derivation to include all observation types * EKF: Add custom ecl::powf function for integer powers * EKF: Convert ekf covariance prediction to use sympy output * EKF: Add test program to compare sympy and matlab covariance prediction Also tests ecl::powf(x,exp) function * EKF: simplify ecl::powf function * Generate code to subfolder generated/ * Add printouts for showing code generation progress * Move generated covariance code to generated folder * Upgrade code generation to python3 * main.py: Remove unused create_symbols function & making code more compact * main.py: move main part into function * Code generation: fix passing wrong rotation matrix to yaw_observation () * EKF: Amend generated code filename for consistency * Move ecl::powf function test to unit tests * EKF: Use updated ecl:powf functionality in test program * Move ecl::powf to utils.hpp * Update ecl::powf test * Update output change indication * test: update expected output for change indicator * test: update expected output for change indicator again Co-authored-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: kamilritz <kritz@ethz.ch>
56 lines
2.2 KiB
Python
56 lines
2.2 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Sat Mar 14 12:47:24 2020
|
|
|
|
@author: roman
|
|
"""
|
|
from sympy import ccode
|
|
from sympy.codegen.ast import float32, real
|
|
|
|
class CodeGenerator:
|
|
def __init__(self, file_name):
|
|
self.file_name = file_name
|
|
self.file = open(self.file_name, 'w')
|
|
|
|
def print_string(self, string):
|
|
self.file.write("// " + string + "\n")
|
|
|
|
def get_ccode(self, expression):
|
|
return ccode(expression, type_aliases={real:float32})
|
|
|
|
def write_subexpressions(self,subexpressions):
|
|
write_string = ""
|
|
for item in subexpressions:
|
|
write_string = write_string + "const float " + str(item[0]) + " = " + self.get_ccode(item[1]) + ";\n"
|
|
|
|
write_string = write_string + "\n\n"
|
|
self.file.write(write_string)
|
|
|
|
def write_matrix(self, matrix, identifier, is_symmetric=False):
|
|
write_string = ""
|
|
|
|
if matrix.shape[0] * matrix.shape[1] == 1:
|
|
write_string = write_string + identifier + " = " + 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"
|
|
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 + "\n\n"
|
|
self.file.write(write_string)
|
|
|
|
def close(self):
|
|
self.file.close()
|