void kalmanUpdate( double *P, double *X, double *C, double *R, double *E,
                   double *K, int n, int m );
void euler2quat( double phi, double theta, double psi, double *Q );
void quat2euler( double *Q, double *phi, double *theta, double *psi );
void propagate_state( double *Q, double *A );
void assemble_A_matrix( double *Q, double *pqr, double *A, double dt_sur_2 );
void propagate_covariance( double *P, double *A , double *Q );
void assemble_CPR_matrix( double *Q, double *CPR );
void assemble_CY_matrix( double *Q, double *CY );
void compute_YPR_vector( double *Q, double *YPR );
void compute_YY_vector( double *Q, double *YY );
void VectorDotProduct( double *V1, double *V2, double *Scalar_Product );
void VectorScale( double *V1, double *K, double *V2 );