def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95): # init vars z = z.reshape((-1, 1)) h = np.zeros(z.shape, dtype=np.float64) H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64) # C functions self.hs[kind](x, extra_args, h) self.Hs[kind](x, extra_args, H) # y is the "loss" y = z - h # if using eskf H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64) self.H_mod(x, H_mod) H = H.dot(H_mod) a = np.linalg.inv(H.dot(P).dot(H.T) + R) maha_dist = y.T.dot(a.dot(y)) if maha_dist > chi2_ppf(maha_thresh, y.shape[0]): return False else: return True
def _update_python(self, x, P, kind, z, R, extra_args=[]): # pylint: disable=dangerous-default-value # init vars z = z.reshape((-1, 1)) h = np.zeros(z.shape, dtype=np.float64) H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64) # C functions self.hs[kind](x, extra_args, h) self.Hs[kind](x, extra_args, H) # y is the "loss" y = z - h # *** same above this line *** if self.msckf and kind in self.Hes: # Do some algebraic magic to decorrelate He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64) self.Hes[kind](x, extra_args, He) # TODO: Don't call a function here, do projection locally A = null(He.T) y = A.T.dot(y) H = A.T.dot(H) R = A.T.dot(R.dot(A)) # TODO If nullspace isn't the dimension we want if A.shape[1] + He.shape[1] != A.shape[0]: self.logger.warning('Warning: null space projection failed, measurement ignored') return x, P, np.zeros(A.shape[0] - He.shape[1]) # if using eskf H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64) self.H_mod(x, H_mod) H = H.dot(H_mod) # Do mahalobis distance test # currently just runs on msckf observations # could run on anything if needed if self.msckf and kind in self.maha_test_kinds: a = np.linalg.inv(H.dot(P).dot(H.T) + R) maha_dist = y.T.dot(a.dot(y)) if maha_dist > chi2_ppf(0.95, y.shape[0]): R = 10e16 * R # *** same below this line *** # Outlier resilient weighting as described in: # "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..." weight = 1 # (1.5)/(1 + np.sum(y**2)/np.sum(R)) S = dot(dot(H, P), H.T) + R / weight K = solve(S, dot(H, P.T)).T I_KH = np.eye(P.shape[0]) - dot(K, H) # update actual state delta_x = dot(K, y) P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T) # inject observed error into state x_new = np.zeros(x.shape, dtype=np.float64) self.err_function(x, delta_x, x_new) return x_new, P, y.flatten()
def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, # pylint: disable=dangerous-default-value maha_test_kinds=[], global_vars=None): # optional state transition matrix, H modifier # and err_function if an error-state kalman filter (ESKF) # is desired. Best described in "Quaternion kinematics # for the error-state Kalman filter" by Joan Sola if eskf_params: err_eqs = eskf_params[0] inv_err_eqs = eskf_params[1] H_mod_sym = eskf_params[2] f_err_sym = eskf_params[3] x_err_sym = eskf_params[4] else: nom_x = sp.MatrixSymbol('nom_x', dim_x, 1) true_x = sp.MatrixSymbol('true_x', dim_x, 1) delta_x = sp.MatrixSymbol('delta_x', dim_x, 1) err_function_sym = sp.Matrix(nom_x + delta_x) inv_err_function_sym = sp.Matrix(true_x - nom_x) err_eqs = [err_function_sym, nom_x, delta_x] inv_err_eqs = [inv_err_function_sym, nom_x, true_x] H_mod_sym = sp.Matrix(np.eye(dim_x)) f_err_sym = f_sym x_err_sym = x_sym # This configures the multi-state augmentation # needed for EKF-SLAM with MSCKF (Mourikis et al 2007) if msckf_params: msckf = True dim_main = msckf_params[0] # size of the main state dim_augment = msckf_params[1] # size of one augment state chunk dim_main_err = msckf_params[2] dim_augment_err = msckf_params[3] N = msckf_params[4] feature_track_kinds = msckf_params[5] assert dim_main + dim_augment * N == dim_x assert dim_main_err + dim_augment_err * N == dim_err else: msckf = False dim_main = dim_x dim_augment = 0 dim_main_err = dim_err dim_augment_err = 0 N = 0 # linearize with jacobians F_sym = f_err_sym.jacobian(x_err_sym) if eskf_params: for sym in x_err_sym: F_sym = F_sym.subs(sym, 0) assert dt_sym in F_sym.free_symbols for i in range(len(obs_eqs)): obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym)) if msckf and obs_eqs[i][1] in feature_track_kinds: obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2])) else: obs_eqs[i].append(None) # collect sympy functions sympy_functions = [] # error functions sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]])) sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]])) # H modifier for ESKF updates sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym])) # state propagation function sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym])) sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym])) # observation functions for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym])) sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym])) if msckf and kind in feature_track_kinds: sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym])) # Generate and wrap all th c code sympy_header, code = sympy_into_c(sympy_functions, global_vars) header = "#pragma once\n" header += "#include \"rednose/helpers/common_ekf.h\"\n" header += "extern \"C\" {\n" pre_code = f"#include \"{name}.h\"\n" pre_code += "\nnamespace {\n" pre_code += "#define DIM %d\n" % dim_x pre_code += "#define EDIM %d\n" % dim_err pre_code += "#define MEDIM %d\n" % dim_main_err pre_code += "typedef void (*Hfun)(double *, double *, double *);\n" if global_vars is not None: for var in global_vars: pre_code += f"\ndouble {var.name};\n" pre_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n" post_code = "\n}\n" # namespace post_code += "extern \"C\" {\n\n" for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: if msckf and kind in feature_track_kinds: He_str = 'He_%d' % kind # ea_dim = ea_sym.shape[0] else: He_str = 'NULL' # ea_dim = 1 # not really dim of ea but makes c function work maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection maha_test = kind in maha_test_kinds pre_code += f"const static double MAHA_THRESH_{kind} = {maha_thresh};\n" header += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);\n" post_code += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {{\n" post_code += f" update<{h_sym.shape[0]}, 3, {int(maha_test)}>(in_x, in_P, h_{kind}, H_{kind}, {He_str}, in_z, in_R, in_ea, MAHA_THRESH_{kind});\n" post_code += "}\n" # For ffi loading of specific functions for line in sympy_header.split("\n"): if line.startswith("void "): # sympy functions func_call = line[5: line.index(')') + 1] header += f"void {name}_{func_call};\n" post_code += f"void {name}_{func_call} {{\n" post_code += f" {func_call.replace('double *', '').replace('double', '')};\n" post_code += "}\n" header += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt);\n" post_code += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt) {{\n" post_code += " predict(in_x, in_P, in_Q, dt);\n" post_code += "}\n" if global_vars is not None: for var in global_vars: header += f"void {name}_set_{var.name}(double x);\n" post_code += f"void {name}_set_{var.name}(double x) {{\n" post_code += f" set_{var.name}(x);\n" post_code += "}\n" post_code += "}\n\n" # extern c funcs = ['f_fun', 'F_fun', 'err_fun', 'inv_err_fun', 'H_mod_fun', 'predict'] func_lists = { 'h': [kind for _, kind, _, _, _ in obs_eqs], 'H': [kind for _, kind, _, _, _ in obs_eqs], 'update': [kind for _, kind, _, _, _ in obs_eqs], 'He': [kind for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds], 'set': [var.name for var in global_vars] if global_vars is not None else [], } # For dynamic loading of specific functions post_code += f"const EKF {name} = {{\n" post_code += f" .name = \"{name}\",\n" post_code += f" .kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs])} }},\n" post_code += f" .feature_kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds])} }},\n" for func in funcs: post_code += f" .{func} = {name}_{func},\n" for group, kinds in func_lists.items(): post_code += f" .{group}s = {{\n" for kind in kinds: str_kind = f"\"{kind}\"" if type(kind) == str else kind post_code += f" {{ {str_kind}, {name}_{group}_{kind} }},\n" post_code += " },\n" post_code += "};\n\n" post_code += f"ekf_init({name});\n" # merge code blocks header += "}" code = "\n".join([pre_code, code, open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read(), post_code]) # write to file if not os.path.exists(folder): os.mkdir(folder) open(os.path.join(folder, f"{name}.h"), 'w').write(header) # header is used for ffi import open(os.path.join(folder, f"{name}.cpp"), 'w').write(code)
def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[], global_vars=None): # pylint: disable=dangerous-default-value # optional state transition matrix, H modifier # and err_function if an error-state kalman filter (ESKF) # is desired. Best described in "Quaternion kinematics # for the error-state Kalman filter" by Joan Sola if eskf_params: err_eqs = eskf_params[0] inv_err_eqs = eskf_params[1] H_mod_sym = eskf_params[2] f_err_sym = eskf_params[3] x_err_sym = eskf_params[4] else: nom_x = sp.MatrixSymbol('nom_x', dim_x, 1) true_x = sp.MatrixSymbol('true_x', dim_x, 1) delta_x = sp.MatrixSymbol('delta_x', dim_x, 1) err_function_sym = sp.Matrix(nom_x + delta_x) inv_err_function_sym = sp.Matrix(true_x - nom_x) err_eqs = [err_function_sym, nom_x, delta_x] inv_err_eqs = [inv_err_function_sym, nom_x, true_x] H_mod_sym = sp.Matrix(np.eye(dim_x)) f_err_sym = f_sym x_err_sym = x_sym # This configures the multi-state augmentation # needed for EKF-SLAM with MSCKF (Mourikis et al 2007) if msckf_params: msckf = True dim_main = msckf_params[0] # size of the main state dim_augment = msckf_params[1] # size of one augment state chunk dim_main_err = msckf_params[2] dim_augment_err = msckf_params[3] N = msckf_params[4] feature_track_kinds = msckf_params[5] assert dim_main + dim_augment * N == dim_x assert dim_main_err + dim_augment_err * N == dim_err else: msckf = False dim_main = dim_x dim_augment = 0 dim_main_err = dim_err dim_augment_err = 0 N = 0 # linearize with jacobians F_sym = f_err_sym.jacobian(x_err_sym) if eskf_params: for sym in x_err_sym: F_sym = F_sym.subs(sym, 0) assert dt_sym in F_sym.free_symbols for i in range(len(obs_eqs)): obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym)) if msckf and obs_eqs[i][1] in feature_track_kinds: obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2])) else: obs_eqs[i].append(None) # collect sympy functions sympy_functions = [] # error functions sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]])) sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]])) # H modifier for ESKF updates sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym])) # state propagation function sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym])) sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym])) # observation functions for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym])) sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym])) if msckf and kind in feature_track_kinds: sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym])) # Generate and wrap all th c code header, code = sympy_into_c(sympy_functions, global_vars) extra_header = "#define DIM %d\n" % dim_x extra_header += "#define EDIM %d\n" % dim_err extra_header += "#define MEDIM %d\n" % dim_main_err extra_header += "typedef void (*Hfun)(double *, double *, double *);\n" extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);" extra_post = "" for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: if msckf and kind in feature_track_kinds: He_str = 'He_%d' % kind # ea_dim = ea_sym.shape[0] else: He_str = 'NULL' # ea_dim = 1 # not really dim of ea but makes c function work maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection maha_test = kind in maha_test_kinds extra_post += """ void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d); } """ % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind) extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh) extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind code += '\nextern "C"{\n' + extra_header + "\n}\n" code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read() code += '\nextern "C"{\n' + extra_post + "\n}\n" if global_vars is not None: global_code = '\nextern "C"{\n' for var in global_vars: global_code += f"\ndouble {var.name};\n" global_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n" extra_header += f"\nvoid set_{var.name}(double x);\n" global_code += '\n}\n' code = global_code + code header += "\n" + extra_header write_code(folder, name, code, header)