static void update_prob( m_elem **P_pre, m_elem **R, m_elem **H, m_elem **P_post, m_elem **K ) { #ifdef PRINT_DEBUG printf( "ekf: updating prob\n" ); #endif #ifdef DIV_DEBUG print_matrix( "P", P_pre, state_size, state_size ); #endif mat_mult( H, P_pre, temp_meas_state, measurement_size, state_size, state_size ); mat_mult_transpose( H, temp_meas_state, temp_meas_meas, measurement_size, state_size, measurement_size ); mat_add( temp_meas_meas, R, temp_meas_meas, measurement_size, measurement_size ); take_inverse( temp_meas_meas, temp_meas_2, measurement_size ); #ifdef DIV_DEBUG print_matrix( "1 / (HPH + R)", temp_meas_2, measurement_size, measurement_size ); #endif mat_transpose_mult( temp_meas_state, temp_meas_2, K, state_size, measurement_size, measurement_size ); /* print_matrix( "Kalman Gain", K, state_size, measurement_size ); */ mat_mult( K, temp_meas_state, temp_state_state, state_size, measurement_size, state_size ); #ifdef PRINT_DEBUG printf( "ekf: updating prob 3\n" ); #endif mat_add( temp_state_state, P_pre, P_post, state_size, state_size ); }
static void update_prob( uFloat **P_pre, uFloat **R, uFloat **H, uFloat **P_post, uFloat **K ) { #ifdef PRINT_DEBUG printf( "ekf: updating prob\n" ); #endif #ifdef DIV_DEBUG printMatrix( "P", P_pre, STATE_SIZE, STATE_SIZE ); #endif #ifdef DIV_DEBUG printMatrix( "H", H, MEAS_SIZE, STATE_SIZE ); #endif #ifdef DIV_DEBUG printMatrix( "R", R, MEAS_SIZE, MEAS_SIZE ); #endif matMultTranspose( P_pre, H, temp_state_meas, STATE_SIZE, STATE_SIZE, MEAS_SIZE ); /* temp_state_meas = P_pre*H' */ matMult( H, temp_state_meas, temp_meas_meas, MEAS_SIZE, STATE_SIZE, MEAS_SIZE ); /* temp_meas_meas = (H*P_pre)*H' */ matAdd( temp_meas_meas, R, temp_meas_meas, MEAS_SIZE, MEAS_SIZE ); take_inverse( temp_meas_meas, temp_meas_2, MEAS_SIZE ); /* temp_meas_2 = inv( H*P_pre*H' + R) */ #ifdef DIV_DEBUG printMatrix( "1 / (HPH + R)", temp_meas_2, MEAS_SIZE, MEAS_SIZE ); #endif matMult( temp_state_meas, temp_meas_2, K, STATE_SIZE, MEAS_SIZE, MEAS_SIZE ); /* K = P_pre * H' * (inv( H*P_pre*H' + R))' */ #ifdef DIV_DEBUG printMatrix( "Kalman Gain", K, STATE_SIZE, MEAS_SIZE ); #endif matMult( H, P_pre, temp_meas_state, MEAS_SIZE, STATE_SIZE, STATE_SIZE ); /* temp_meas_state = H * P_pre */ matMult( K, temp_meas_state, temp_state_state, STATE_SIZE, MEAS_SIZE, STATE_SIZE ); /* temp_state_state = K*H*P_pre */ #ifdef PRINT_DEBUG printf( "ekf: updating prob 3\n" ); #endif matSub( P_pre, temp_state_state, P_post, STATE_SIZE, STATE_SIZE ); /* P_post = P_pre - K*H*P_pre */ #ifdef DIV_DEBUG printMatrix( "New P_post", P_post, STATE_SIZE, STATE_SIZE ); #endif }