You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

56 lines
2.2 KiB

EKF: replacement of covariance prediction autocode with sympy generated output (#870) * 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>
5 years ago
#!/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()