void kalman_2D_prediction(kalman_filter_2D_t *kalman, vector_2_t control)
{
kalman->state = vadd2( mvmul2(kalman->system_model, kalman->state),
mvmul2(kalman->control_model, control));
kalman->covariance= madd2( mmul2( mmul2( kalman->system_model,
kalman->covariance),
trans2(kalman->system_model)),
kalman->noise_prediction);
}
void kalman_2D_update(kalman_filter_2D_t *kalman, vector_2_t measurement)
{
vector_2_t innovation = vsub2( measurement,
mvmul2(kalman->observation_model, kalman->state));
matrix_2x2_t innovation_covariance = madd2( mmul2( mmul2(kalman->observation_model, kalman->covariance),
trans2(kalman->observation_model)),
kalman->noise_measurement);
matrix_2x2_t kalman_gain = mmul2( mmul2(kalman->covariance, trans2(kalman->observation_model)),
inv2(innovation_covariance));
kalman->state = vadd2(kalman->state, mvmul2(kalman_gain, innovation));
kalman->covariance = mmul2( msub2(ident_2x2, mmul2(kalman_gain, kalman->observation_model)),
kalman->covariance);
}
#include "print_util.h"
bool run_matrix_tests(void)
{
matrix_2x2_t m2={.v={{1, 2},{3,2}}};
print_util_print_matrix(print_util_get_debug_stream(), m2.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2t=trans2(m2);
print_util_print_matrix(print_util_get_debug_stream(), m2t.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2i=inv2(m2);
print_util_print_matrix(print_util_get_debug_stream(), m2i.v[0], 2, 2, 2); print_util_dbg_print("\n");
print_util_print_matrix(print_util_get_debug_stream(), madd2(m2, m2t).v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2_=mmul2(m2, m2i);
print_util_print_matrix(print_util_get_debug_stream(), m2_.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_3x3_t m3={.v={{1, 2, 4},{2,4,2},{4,2,1}}};
print_util_print_matrix(print_util_get_debug_stream(), m3.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3d=diag_3x3(row3(m3, 0));
print_util_print_matrix(print_util_get_debug_stream(), m3d.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3t=trans3(m3);
print_util_print_matrix(print_util_get_debug_stream(), m3t.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3i=inv3(m3);
print_util_print_matrix(print_util_get_debug_stream(), m3i.v[0], 3, 3, 2); print_util_dbg_print("\n");