Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 838 → Rev 967

/branches/KalmanFilter MikeW/kafi.c
1,15 → 1,735
/*
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);
}
 
/*****************************************************************************
VARIABLES
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 ---*/
}