Subversion Repositories FlightCtrl

/*

Copyright 2008, by Michael Walter

All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser

General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but

WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public

License along with this program. If not, see <http://www.gnu.org/licenses/>.

Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that

are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de

unless it is stated otherwise.

*/

/*****************************************************************************

  INCLUDES

**************************************************************************** */

#include "kafi.h"

#include "analog.h"

#include "main.h"

#include "FlightControl.h"

#include "mm3.h"

/*****************************************************************************

(SYMBOLIC) CONSTANTS

*****************************************************************************/

/*****************************************************************************

  VARIABLES

*****************************************************************************/

volatile int  KompassValue = 0;

/* Kalman filter */

f32_t Phi, Theta, sinPhi,  cosPhi, tanTheta, secTheta;

f32_t sinPhi, cosPhi, sinTheta, cosTheta;

f32_t sinPhi_P,  cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P;

f32_t **matPs;  /*  Predicted system Noise */

f32_t **matP;    /*  Predicted system Noise */

f32_t **matP_tmp;

f32_t **matQ;      /* system noise covariance */

f32_t **matR;       /* measurement noise covariance */

f32_t **matXd;    /* estimated state */

f32_t **matXs;    /* predicted state */

f32_t **matX_tmp1;

f32_t **matX_tmp2;

f32_t **matPhi;   /* transition matrix to predict new state from current state */

f32_t **matB ;     /* control matrix to predict new state from ext. control vector U */

f32_t **matU;     /* control vector */

f32_t **matC;    /* Jacobi matrix */

f32_t **matY;     /* real measurements */

f32_t **matYs;    /* predicted measurements */

f32_t **matdY;    /* innovation */

f32_t **temp_meas_meas;

f32_t **temp_meas_state;

f32_t **temp_state_meas;

f32_t **temp_meas_2;

f32_t **temp_state_state;

f32_t **matK; /* the Kalman gain matrix  */

char fYValid[KAFI_DIM_Y];   /* measurement validity flag */

/* ****************************************************************************

Functionname:     AttitudeEstimation                      */ /*!

Description:         Main Kalman Filter Loop

@param[in]        

  @return           void

  @pre              -

  @post             -

  @author          Michael Walter

**************************************************************************** */

void FlightAttitudeEstimation()

{

  /* Predict the Measurent */

  trPredictMeasurement();

  /* Update the Jacobi Matrix */

  trUpdateJacobiMatrix();

  /*  Extract measurements and store them in vector matY.

  Measurement validity is indicated by fYValid[] */

  trMeasure();

  /* Innovation: calculate Xd, and Pd  */

  trInnovate();

  /* Update transition matrix Phi  */

  //trUpdatePhi();  // Is an Identity matrix which will not change

  /* Update control matrix B */

  trUpdateBU();

  /* Predict new state Xs and Ps */

  KAFIPrediction();

  /* store innovated state vector in current state */

  matGetFull(matXs, _Phi   , 0, &status.Phi);

  matGetFull(matXs, _Theta , 0, &status.Theta);

  matGetFull(matXs, _Psi   , 0, &status.Psi);

  /* Limit Angles to [-2Pi;...;+2Pi] */

  trLimitAngles();

}

/* ****************************************************************************

Functionname:     Kafi_Init                      */ /*!

Description:      Generate Kalman Matrizes  

@param[in]        

  @return           void

  @pre              -

  @post             -

  @author          Michael Walter

**************************************************************************** */

void Kafi_Init()

{

  /* create kalman filter and associated matrizes */

  ui32_t i, j;

  matPs = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X );

  matQ = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);

  matR = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_Y);

  matXd = matrix( 1, KAFI_DIM_X, 1,1);    /* estimated state */

  matXs = matrix( 1, KAFI_DIM_X, 1,1);    /* predicted state */

  matX_tmp1 = matrix( 1, KAFI_DIM_X,1, 1);

  matX_tmp2 = matrix( 1, KAFI_DIM_X, 1,1);

  matPhi = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);/* transition matrix to predict new state from current state */

  matB = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_U); /* control matrix to predict new state from ext. control vector U */

  matU = matrix( 1, KAFI_DIM_U, 1,1);   /* control vector */

  matC = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_X);    /* Jacobi matrix */

  matY = matrix( 1, KAFI_DIM_Y, 1,1);   /* real measurements */

  matdY = matrix( 1, KAFI_DIM_Y, 1,1);    /* innovation */

  matYs = matrix( 1, KAFI_DIM_Y, 1,1);    /* predicted measurements */

  matP = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);

  matK = matrix(1, KAFI_DIM_X, 1, KAFI_DIM_Y);

  temp_state_state = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X );

  temp_meas_state = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_X );

  temp_state_meas = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_Y );

  temp_meas_meas = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y );

  temp_meas_2 = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y );

  matP_tmp = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X);

  /* set phi to identity matrix */

  for ( j = 0; j < KAFI_DIM_X; j++ )

  {

    for ( i = 0; i < KAFI_DIM_X; i++ )

    {

      matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F);

    }

  }

  /* set diagonal of measurement noise covariance R */

  matSetDiagonal(matR, _ax, _ax, 10.0F);

  matSetDiagonal(matR, _ay, _ay, 10.0F);

  matSetDiagonal(matR, _az, _az, 10.0F);

  matSetDiagonal(matR, _compass, _compass, 5.0F);

#ifdef MELEXIS_GYRO

  /* set diagonal of system noise covariance Q, dim(X,X) */

  matSetDiagonal(matQ, _Phi, _Phi, 0.000006F);

  matSetDiagonal(matQ, _Theta, _Theta, 0.000006F);

  matSetDiagonal(matQ, _Psi, _Psi, 0.000006F);

#else

  /* set diagonal of system noise covariance Q */

  matSetDiagonal(matQ, _Phi, _Phi, 0.00001F);

  matSetDiagonal(matQ, _Theta, _Theta, 0.00001F);

  matSetDiagonal(matQ, _Psi, _Psi, 0.00005F);

#endif

  /* set diagonal init. estimation error (covariance) P0 */

  matSetDiagonal(matPs, _Phi, _Phi, 0.00001F);

  matSetDiagonal(matPs, _Theta, _Theta, 0.00001F);

  matSetDiagonal(matPs, _Psi, _Psi, 0.00001F);

  matSetFull(matXd, _Phi, 0, 0.0F);

  matSetFull(matXd, _Theta, 0, 0.0F);

  matSetFull(matXd, _Psi, 0, 0.0F);

  matSetFull(matXs, _Phi, 0, 0.0F);

  matSetFull(matXs, _Theta, 0, 0.0F);

  matSetFull(matXs, _Psi, 0, 0.0F);

}

/* ****************************************************************************

Functionname:    KAFIPrediction                      */ /*!

Description:  Predict new state Xs and Ps    

  X_k = A * X_(k-1) + B * u_k

  P_k = A * P_(k-1) * A^T + Q

@param[in]        

  @return           void

  @pre              -

  @post             -

  @author           Michael Walter

**************************************************************************** */

void KAFIPrediction()

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*--- VARIABLES ---*/

  /* Innovation: calculate Xd, and Pd  */

  /* X_k = A * X_(k-1) + B * u_k */

  /* matX_tmp1 = matPhi * matXd */

  /*

  matrix_mult(matPhi,matXd, matX_tmp1,

  KAFI_DIM_X, KAFI_DIM_X, 1);

  */

  /* matX_tmp2 = matB * matU */

  matrix_mult( matB,matU, matX_tmp2,

    KAFI_DIM_X, KAFI_DIM_U, 1);

  /* matXs = matPhi * matXd + matB * matU */

  /*

  matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 );

  matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 );

  */

  matXs[1][1] = matXd[1][1] + matX_tmp2[1][1];

  matXs[2][1] = matXd[2][1] + matX_tmp2[2][1];

  matXs[3][1] = matXd[3][1] + matX_tmp2[3][1];

  /*P_k = A * P_(k-1)*A^T + Q */

  /*

  matrix_mult_transpose( matP, matPhi, matP_tmp,  // Can be skiped PHI is a ident matrix

  KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X );

  matrix_mult( matPhi, matP_tmp, matP,

  KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X );

  matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X );

  */

  matPs[1][1] = matP[1][1] + matQ[1][1];

  matPs[2][2] = matP[2][2] + matQ[2][2];

  matPs[3][3] = matP[3][3] + matQ[3][3];

}

/* ****************************************************************************

Functionname:   trInnovate                        */ /*!

Description:  Innovation: calculate Xd, and Pd    

  K = Ps * C^T * (C * Ps * C^T + R)^-1

  X = Xs + K (Y - Ys)

@param[in]        

  @return           void

  @pre              -

  @post             -

  @author           Michael Walter

**************************************************************************** */

void trInnovate()

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*--- VARIABLES ---*/

#if 0  /* Runtime optimised. To be verified */

  /* temp_meas_state = matC * matPs */

  matrix_mult( matC, matPs, temp_meas_state,

   KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X );

  /* temp_meas_meas = matC * matPs^T * matC^T */

  matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas,

    KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y );

  /* temp_meas_meas = matC * matPs^T * matC^T + matR */

  /*

  matrix_add( temp_meas_meas, matR, temp_meas_meas,

  KAFI_DIM_Y, KAFI_DIM_Y );

  */

  temp_meas_meas[1][1] += matR[1][1];

  temp_meas_meas[2][2] += matR[2][2];

  temp_meas_meas[3][3] += matR[3][3];

  temp_meas_meas[4][4] += matR[4][4];

  /* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */

  take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y );

  /* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */

  matrix_transpose_mult( temp_meas_state, temp_meas_2, matK,

    KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y );

  /* temp_state_state = matK * matC * matPs */

  matrix_mult( matK, temp_meas_state, temp_state_state,

    KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X );

  /* matP = matPs - matK * matC * matPs  */

  matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X );

#else

  /* temp_state_meas = matPs * matC^T */

  matrix_mult_transpose( matPs, matC, temp_state_meas,

    KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y );

  /* temp_meas_meas = matC * matPs * matC^T */

  matrix_mult( matC, temp_state_meas, temp_meas_meas,

    KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y );

  /* temp_meas_meas = matC * matPs * matC^T + matR */

  /*

  matrix_add( temp_meas_meas, matR, temp_meas_meas,

  KAFI_DIM_Y, KAFI_DIM_Y );

  */

  temp_meas_meas[1][1] += matR[1][1];

  temp_meas_meas[2][2] += matR[2][2];

  temp_meas_meas[3][3] += matR[3][3];

  temp_meas_meas[4][4] += matR[4][4];

  /* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */

  take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y );

  /* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */

  matrix_mult( temp_state_meas, temp_meas_2, matK,

    KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y );

  /* temp_meas_state = matC * matPs*/

  matrix_mult( matC, matPs, temp_meas_state,

    KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X );

  /* temp_state_state = matK * matC * matPs */

  matrix_mult( matK, temp_meas_state, temp_state_state,

    KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X );

  /* matP = matPs - matK * matC * matPs */

  matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X );

#endif

  /* matdY = matY - matYs */

  /*

  matrix_sub( matY, matYs, matdY,

    KAFI_DIM_Y, 1 );

  */

  matdY[1][1] = matY[1][1] - matYs[1][1];

  matdY[2][1] = matY[2][1] - matYs[2][1];

  matdY[3][1] = matY[3][1] - matYs[3][1];

  /* We will only update the Heading if we got a valid compass signal */

  if(fYValid[_compass] == 1)

  {

    matdY[4][1] = matY[4][1] - matYs[4][1];

  }

  else

  {

    matdY[4][1] = 0.0F;

  }

  /* matX_tmp1 = matK * (Y -Ys) */

  matrix_mult( matK, matdY, matX_tmp1,

    KAFI_DIM_X, KAFI_DIM_Y, 1 );

 /* matXd = matXs + matK * (Y -Ys) */

  /*

  matrix_add( matXs, matX_tmp1, matXd,

    KAFI_DIM_X, 1 );

  */

  matXd[1][1] = matXs[1][1] + matX_tmp1[1][1];

  matXd[2][1] = matXs[2][1] + matX_tmp1[2][1];

  matXd[3][1] = matXs[3][1] + matX_tmp1[3][1];

  matXd[4][1] = matXs[4][1] + matX_tmp1[4][1];

}

/* ****************************************************************************

Functionname:     trUpdatePhi                                          */ /*!

Description:      Setup matPhi, used to calculate the predicted state

from the current state

@param[in]        

@param[in]        fCycleTime

  @return           void

  @pre              -

  @post             -

  @author           Michael Walter

**************************************************************************** */

void trUpdatePhi()

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*--- VARIABLES ---*/

  matSetFull(matPhi, _Phi, _Phi, 1.0F);

  matSetFull(matPhi, _Theta, _Theta, 1.0F);

  matSetFull(matPhi, _Psi, _Psi, 1.0F);

}

/* ****************************************************************************

Functionname:     trUpdateJacobiMatrix                      */ /*!

Description:

@param[in]        void

  @return           void

  @pre              -

  @post             -

  @author           Michael Walter

*****************************************************************************/

void trUpdateJacobiMatrix(void)

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*

  Simplified Version d/dt

  Pre_ax =  du - sinTheta * g;                

  Pre_ay =  dv + cosTheta_sinPhi * g;    

  Pre_az =  dw + cosTheta_cosPhi * g;      

  */

  /*--- VARIABLES ---*/

  f32_t Phi, Theta, Psi;

  f32_t g = 9.81F;

  Phi   = status.Phi;

  Theta = status.Theta;

  Psi   = status.Psi;

  sinPhi = sin(Phi);

  cosPhi = cos(Phi);

  sinTheta = sin(Theta);

  cosTheta = cos(Theta);

  //Pre_ax =  du - sinTheta * g;                

  //matSetFull(matC, _ax, _Phi   , 0);

  matSetFull(matC, _ax, _Theta, -cosTheta * g);

  //matSetFull(matC, _ax, _Psi   , 0);

  //Pre_ay =  dv + cosTheta_sinPhi * g;    

  matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g);

  matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g);

  //matSetFull(matC, _ay, _Psi   , 0);

  //Pre_az =  dw + cosTheta_cosPhi * g;  

  matSetFull(matC, _az, _Phi   , -cosTheta * sinPhi * g);

  matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g);

  //matSetFull(matC, _az, _Psi   , 0);

  matSetFull(matC, _compass, _Psi , 1.0F);

}

/*****************************************************************************

Functionname:     trPredictMeasurement                      */ /*!

Description:Predict Measurements: AX, AY, AZ, Compass

@param[in]        void

  @return           void

  @pre              -

  @post             -

  @author           Michael Walter

*****************************************************************************/

void trPredictMeasurement(void)

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*

  Simplified Version

  Pre_ax =  du - sinTheta * g;    

  Pre_ay =  dv + cosTheta_sinPhi * g;    

  Pre_az =  dw + cosTheta_cosPhi * g;  

  */

  /*--- VARIABLES ---*/

  f32_t g = 9.81F;

  f32_t Pre_ax, Pre_ay,  Pre_az;    

  f32_t Phi, Theta, Psi;

  matGetFull(matXs, _Phi, 0, &Phi);

  matGetFull(matXs, _Theta, 0, &Theta);

  matGetFull(matXs, _Psi, 0, &Psi);

  sinPhi_P = sin(Phi);

  cosPhi_P = cos(Phi);

  sinTheta_P = sin(Theta);

  cosTheta_P = cos(Theta);

  /* Simplified Version */

  Pre_ax =  -sinTheta_P * g;    

  Pre_ay =  cosTheta_P * sinPhi_P * g;    

  Pre_az =  cosTheta_P * cosPhi_P * g;  

  matSetFull(matYs, _ax, 0, Pre_ax);

  matSetFull(matYs, _ay, 0, Pre_ay);

  matSetFull(matYs, _az, 0, Pre_az);

  matSetFull(matYs, _compass, 0, Psi);

}

/* ****************************************************************************

Functionname:     trMeasure                      */ /*!

Description:      

  @param[in]        

@return           void

@pre              -

@post             -

@author           Michael Walter

**************************************************************************** */

void trMeasure()

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*--- VARIABLES ---*/

#ifdef USE_COMPASS  

  static int updCompass = 10;

  f32_t Psi, Compass;

#endif 

  f32_t ADC_ax = 0.0F;

  f32_t ADC_ay = 0.0F;

  f32_t ADC_az = 0.0F;

  fYValid[_ax] = 1;

  fYValid[_ay] = 1;

  fYValid[_az] = 1;

#ifdef INTERNAL_REFERENCE 

  ADC_ax = AdWertAccNick * 0.044F;

  ADC_ay = -AdWertAccRoll * 0.044F;

  ADC_az = AdWertAccHoch * 0.044F;

#else

  ADC_ax = AdWertAccNick * 0.047F;

  ADC_ay = -AdWertAccRoll * 0.047F;

  ADC_az = AdWertAccHoch * 0.047F;

#endif

  matSetFull(matY, _ax, 0, ADC_ax);

  matSetFull(matY, _ay, 0, ADC_ay);

  matSetFull(matY, _az, 0, ADC_az);

#ifdef USE_COMPASS

  if (!updCompass--)

  {

    updCompass = 10; // update only at 2ms*50 = 100ms (10Hz)

    KompassValue = MM3_Heading();

  }

  Compass = KompassValue / 180.F * PI;

  matGetFull(matXs, _Psi, 0, &Psi);

  if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi))

  {

    Compass += 2 * PI;

  }

  else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi))

  {

    Compass -= 2 * PI;

  }

  matSetFull(matY, _compass, 0, Compass);

  /* Roll and Yaw angle are smaller 8 Deg*/

  if ((abs(status.iPhi10)  < 80 ) && (abs(status.iTheta10) < 80))

  {

    fYValid[_compass] = 1;

  }

  else

  {

    fYValid[_compass] = 0;

  }

#else

  fYValid[_compass] = 0;

#endif

}

/* ****************************************************************************

Functionname:     trUpdateBU                      */ /*!

Description:      Update control matrix B and vector U

@param[in]        

  @return           void

  @pre              -

  @post             -

  @author          Michael Walter

**************************************************************************** */

void trUpdateBU()

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*--- VARIABLES ---*/

  /*

  dPhi      | 1      sinPhi_tanTheta    cosPhi_tanTheta  |   p  

  dTheta =  | 0      cosPhi                     -sinPhi  | * q

  dPsi      | 0      sinPhi_secTheta    cosPhi_secTheta  |   w

  */

  static f32_t fSumGier = 0.0F;

  static f32_t fSumNick = 0.0F;

  static f32_t fSumRoll = 0.0F;

  f32_t ADC_p = 0.0F;

  f32_t ADC_q = 0.0F;

  f32_t ADC_r = 0.0F;

  /* store predicted state vector in current state */

  matGetFull(matXd, _Phi   , 0, &Phi);

  matGetFull(matXd, _Theta , 0, &Theta);

  sinPhi = sin(Phi);

  cosPhi = cos(Phi);

  tanTheta = tan(Theta);

  secTheta = 1.0F / cos(Theta);

  matSetFull(matB, _Phi, _p, 1.0F);

  matSetFull(matB, _Phi, _q, sinPhi * tanTheta );

  matSetFull(matB, _Phi, _r, cosPhi * tanTheta );

  //matSetFull(matB, _Theta, _p, 0.0F);

  matSetFull(matB, _Theta, _q, cosPhi );

  matSetFull(matB, _Theta, _r, -sinPhi );

  //matSetFull(matB, _Psi, _p, 0.0F);

  matSetFull(matB, _Psi, _q, sinPhi * secTheta );

  matSetFull(matB, _Psi, _r, cosPhi * secTheta );  

  ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime;

  ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime;

  ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime;

  fSumGier += ADC_r * 180.F / 3.14F;

  fSumNick += ADC_q * 180.F / 3.14F;

  fSumRoll += ADC_p * 180.F / 3.14F;

  DebugOut.Analog[3] = fSumNick;

  DebugOut.Analog[4] = fSumRoll;

  DebugOut.Analog[5] = fSumGier;

  matSetFull(matU, _p, 0, ADC_p );

  matSetFull(matU, _q, 0, ADC_q);

  matSetFull(matU, _r, 0, ADC_r);

}

/* ****************************************************************************

  Functionname:     trLimitAngles                      */ /*!

  Description:      

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author          

**************************************************************************** */

void trLimitAngles()

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*--- VARIABLES ---*/

  f32_t Psi;

  if (status.Phi > 2.0F * PI)

  {

    status.Phi -= 2.0F * PI;

    matSetFull(matXs,  _Phi, 0, status.Phi);

  }

  if (status.Phi < -2.0F * PI)

  {

    status.Phi += 2.0F * PI;

    matSetFull(matXs,  _Phi,  0, status.Phi);    

  }

  if (status.Theta > 2.0F * PI)

  {

    status.Theta -= 2.0F * PI;

    matSetFull(matXs, _Theta, 0, status.Theta);

  }

  if (status.Theta < -2.0F * PI)

  {

    status.Theta += 2.0F * PI;

    matSetFull(matXs, _Theta, 0, status.Theta);

  }

  if (status.Psi > 2.0F * PI)

  {

    status.Psi -= 2.0F * PI;

    sollGier -= 3600;

    matGetFull(matXs, _Psi, 0, &Psi);

    matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI);

    matGetFull(matXd, _Psi, 0, &Psi);

    matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI);

  }

  if (status.Psi < 0.0F * PI)

  {

    status.Psi += 2.0F * PI;

    sollGier += 3600;

    matGetFull(matXs, _Psi, 0, &Psi);

    matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI);

    matGetFull(matXd, _Psi, 0, &Psi);

    matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI);

  }

  status.iPhi10 = (int) (status.Phi * 573.0F);

  status.iTheta10 = (int) (status.Theta * 573.0F);

  status.iPsi10 = (int) (status.Psi * 573.0F);

  if ((sollGier - status.iPsi10) > 3600)

  {

    sollGier -= 3600;

  }

  if ((sollGier - status.iPsi10) < -3600)

  {

    sollGier += 3600;

  }

}

/* ****************************************************************************

  Functionname:     trEstimateVelocity                      */ /*!

  Description:      

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author          

**************************************************************************** */

void trEstimateVelocity()

{

  /*--- (SYMBOLIC) CONSTANTS ---*/

  /*--- VARIABLES ---*/

}