Esempi in C++ (Cpp) per apply_measurement

Esempio n. 1

File: kalman.c Progetto: Colyette/P15230

static void update_system( m_elem *z, m_elem *x_pre,
             m_elem **K, m_elem *x_post )
{
#ifdef PRINT_DEBUG
  printf( "ekf: updating system\n" );
#endif

  apply_measurement( x_pre, z_estimate );
  vec_sub( z, z_estimate, z_estimate, measurement_size );
  mat_mult_vector( K, z_estimate, x_post, state_size, measurement_size );
  vec_add( x_post, x_pre, x_post, state_size );
}

Esempio n. 2

File: kalman.c Progetto: orbswarm/orbswarm-svn

static void update_system( uFloat *z, uFloat *x_pre,
			 uFloat **K, uFloat *x_post )
{
#ifdef PRINT_DEBUG
  printf( "ekf: updating system\n" );
#endif

  apply_measurement( x_pre, z_estimate ); /* z_estimate = h ( x_pre ) */
  vecSub( z, z_estimate, z_estimate, MEAS_SIZE ); /* z_estimate = z - z_estimate */
  matMultVector( K, z_estimate, x_post, STATE_SIZE, MEAS_SIZE ); /* x_post = K * (z- z_estimate) */
  vecAdd( x_post, x_pre, x_post, STATE_SIZE ); /* x_post + x_pre + K * (z-z_estimate) */
}