C++ (Cpp) take_inverse Exemples

Exemple #1

Fichier : kalman.c Projet : Colyette/P15230

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 );
}

Exemple #2

Fichier : kalman.c Projet : orbswarm/orbswarm-svn

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
}