Example #1
0
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);
}
Example #2
0
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);
}
Example #3
0
#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");