Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* EKF sym (for live_kf only) in c++ * Cython wrapper for EKFSym in cpp * Linking cython wrapper * Compiling with eigency * Non failing version * Correct passing data from Eigen to numpy * Working for locationd without rewidning * Rewinding * Fixed relative imports * Pass data by reference * Dynamic shared lib loading * Both working python and c++ ekfsym * Fixed names * Naming error * set globals * Work with paramsd * Fixed working on paramsd * Added test to compare ekf_sym_py and ekf_sym_pyx * Fixed imports * Dockerfile fix * Cleanup * cleanup * CI fixes * No longer use output parameters * CI * Single sconscript for rednose and locationd/models * cleanup * CI fix * removed unimplemented functions from header * loading functions * header file generation * code generation fix * formatting
- Loading branch information
Showing
23 changed files
with
1,008 additions
and
120 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,125 @@ | ||
#!/usr/bin/env python3 | ||
import os | ||
import sys | ||
import sympy as sp | ||
import numpy as np | ||
import unittest | ||
|
||
if __name__ == '__main__': # generating sympy code | ||
from rednose.helpers.ekf_sym import gen_code | ||
else: | ||
from rednose.helpers.ekf_sym_pyx import EKF_sym | ||
from rednose.helpers.ekf_sym import EKF_sym as EKF_sym2 | ||
|
||
|
||
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated')) | ||
|
||
|
||
class ObservationKind: | ||
UNKNOWN = 0 | ||
NO_OBSERVATION = 1 | ||
POSITION = 1 | ||
|
||
names = [ | ||
'Unknown', | ||
'No observation', | ||
'Position' | ||
] | ||
|
||
@classmethod | ||
def to_string(cls, kind): | ||
return cls.names[kind] | ||
|
||
|
||
class States: | ||
POSITION = slice(0, 1) | ||
VELOCITY = slice(1, 2) | ||
|
||
|
||
class CompareFilter: | ||
name = "compare" | ||
|
||
initial_x = np.array([0.5, 0.0]) | ||
initial_P_diag = np.array([1.0**2, 1.0**2]) | ||
Q = np.diag([0.1**2, 2.0**2]) | ||
obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)} | ||
|
||
@staticmethod | ||
def generate_code(generated_dir): | ||
name = CompareFilter.name | ||
dim_state = CompareFilter.initial_x.shape[0] | ||
|
||
state_sym = sp.MatrixSymbol('state', dim_state, 1) | ||
state = sp.Matrix(state_sym) | ||
|
||
position = state[States.POSITION, :][0,:] | ||
velocity = state[States.VELOCITY, :][0,:] | ||
|
||
dt = sp.Symbol('dt') | ||
state_dot = sp.Matrix(np.zeros((dim_state, 1))) | ||
state_dot[States.POSITION.start, 0] = velocity | ||
f_sym = state + dt * state_dot | ||
|
||
obs_eqs = [ | ||
[sp.Matrix([position]), ObservationKind.POSITION, None], | ||
] | ||
|
||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state) | ||
|
||
def __init__(self, generated_dir): | ||
dim_state = self.initial_x.shape[0] | ||
dim_state_err = self.initial_P_diag.shape[0] | ||
|
||
# init filter | ||
self.filter_py = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err) | ||
self.filter_pyx = EKF_sym2(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err) | ||
|
||
def get_R(self, kind, n): | ||
obs_noise = self.obs_noise[kind] | ||
dim = obs_noise.shape[0] | ||
R = np.zeros((n, dim, dim)) | ||
for i in range(n): | ||
R[i, :, :] = obs_noise | ||
return R | ||
|
||
|
||
class TestCompare(unittest.TestCase): | ||
def test_compare(self): | ||
np.random.seed(0) | ||
|
||
kf = CompareFilter(GENERATED_DIR) | ||
|
||
# Simple simulation | ||
dt = 0.01 | ||
ts = np.arange(0, 5, step=dt) | ||
xs = np.empty(ts.shape) | ||
|
||
# Simulate | ||
x = 0.0 | ||
for i, v in enumerate(np.sin(ts * 5)): | ||
xs[i] = x | ||
x += v * dt | ||
|
||
# insert late observation | ||
switch = (20, 40) | ||
ts[switch[0]], ts[switch[1]] = ts[switch[1]], ts[switch[0]] | ||
xs[switch[0]], xs[switch[1]] = xs[switch[1]], xs[switch[0]] | ||
|
||
for t, x in zip(ts, xs): | ||
# get measurement | ||
meas = np.random.normal(x, 0.1) | ||
z = np.array([[meas]]) | ||
R = kf.get_R(ObservationKind.POSITION, 1) | ||
|
||
# Update kf | ||
kf.filter_py.predict_and_update_batch(t, ObservationKind.POSITION, z, R) | ||
kf.filter_pyx.predict_and_update_batch(t, ObservationKind.POSITION, z, R) | ||
|
||
self.assertAlmostEqual(kf.filter_py.get_filter_time(), kf.filter_pyx.get_filter_time()) | ||
self.assertTrue(np.allclose(kf.filter_py.state(), kf.filter_pyx.state())) | ||
self.assertTrue(np.allclose(kf.filter_py.covs(), kf.filter_pyx.covs())) | ||
|
||
|
||
if __name__ == "__main__": | ||
generated_dir = sys.argv[2] | ||
CompareFilter.generate_code(generated_dir) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,37 @@ | ||
Import('env', 'envCython', 'arch', 'rednose_config') | ||
|
||
generated_folder = rednose_config['generated_folder'] | ||
|
||
templates = Glob('#rednose/templates/*') | ||
|
||
sympy_helpers = "#rednose/helpers/sympy_helpers.py" | ||
ekf_sym = "#rednose/helpers/ekf_sym.py" | ||
ekf_sym_pyx = "#rednose/helpers/ekf_sym_pyx.pyx" | ||
ekf_sym_cc = "#rednose/helpers/ekf_sym.cc" | ||
common_ekf = "#rednose/helpers/common_ekf.cc" | ||
|
||
found = {} | ||
for target, (command, combined_lib) in rednose_config['to_build'].items(): | ||
if File(command).exists(): | ||
found[target] = (command, combined_lib) | ||
|
||
lib_target = [common_ekf] | ||
for target, (command, combined_lib) in found.items(): | ||
target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h']) | ||
command_file = File(command) | ||
|
||
env.Command(target_files, | ||
[templates, command_file, sympy_helpers, ekf_sym], | ||
command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath()) | ||
|
||
if combined_lib: | ||
lib_target.append(target_files[0]) | ||
else: | ||
env.SharedLibrary(f'{generated_folder}/' + target, [target_files[0], common_ekf]) | ||
|
||
libkf = env.SharedLibrary(f'{generated_folder}/libkf', lib_target) | ||
|
||
lenv = envCython.Clone() | ||
lenv["LINKFLAGS"] += [libkf[0].get_labspath()] | ||
ekf_sym_so = lenv.Program('#rednose/helpers/ekf_sym_pyx.so', [ekf_sym_pyx, ekf_sym_cc, common_ekf]) | ||
lenv.Depends(ekf_sym_so, libkf) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,19 @@ | ||
#include "common_ekf.h" | ||
|
||
std::vector<const EKF*>& get_ekfs() { | ||
static std::vector<const EKF*> vec; | ||
return vec; | ||
} | ||
|
||
void ekf_register(const EKF* ekf) { | ||
get_ekfs().push_back(ekf); | ||
} | ||
|
||
const EKF* ekf_lookup(const std::string& ekf_name) { | ||
for (const auto& ekfi : get_ekfs()) { | ||
if (ekf_name == ekfi->name) { | ||
return ekfi; | ||
} | ||
} | ||
return NULL; | ||
} |
Oops, something went wrong.