Esempio n. 1
0
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++;
}
Esempio n. 2
0
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++;
}