void iter_ext_kalman_step( m_elem *z_in )
{
int iteration = 1;
m_elem est_change;
m_elem *prev_state;
m_elem *new_state;
m_elem *temp;
generate_system_transfer( state_pre, sys_transfer );
estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
apply_system( state_post, state_pre );
/* Now iterate, updating the probability and the system model
until no change is noticed between iteration steps */
prev_state = iter_state0;
new_state = iter_state1;
generate_measurement_transfer( state_pre, mea_transfer );
update_prob( cov_pre, mea_noise_cov, mea_transfer,
cov_post, kalman_gain );
update_system( z_in, state_pre, kalman_gain, prev_state );
est_change = calc_state_change( state_pre, prev_state );
while( (est_change < ITERATION_THRESHOLD) &&
(iteration++ < ITERATION_DIVERGENCE) )
{
#ifdef DEBUG_ITER
print_vector( "\titer state", prev_state, 10 );
#endif
/* Update the estimate */
generate_measurement_transfer( prev_state, mea_transfer );
update_prob( cov_pre, mea_noise_cov, mea_transfer,
cov_post, kalman_gain );
update_system( z_in, prev_state, kalman_gain, new_state );
est_change = calc_state_change( prev_state, new_state );
temp = prev_state;
prev_state = new_state;
new_state = temp;
}
vec_copy( prev_state, state_post, state_size );
#ifdef PRINT_DEBUG
printf( "iekf: step %3d, %2d iters\n", global_step, iteration );
#endif
global_step++;
}
void extended_kalman_step( uFloat *z_in )
{
#ifdef PRINT_DEBUG
printf( "ekf: step %d\n", global_step );
#endif
#ifdef PRINT_DEBUG
printVector( "measurements ", z_in, MEAS_SIZE );
#endif
/***************** Gain Loop *****************
First, linearize locally, then do normal gain loop */
generate_system_transfer( state_pre, sys_transfer );
generate_measurement_transfer( state_pre, mea_transfer );
estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );
/************** Estimation Loop ***************/
rungeKutta( state_post, state_pre );
update_system( z_in, state_pre, kalman_gain, state_post );
global_step++;
}