Subversion Repositories FlightCtrl

Rev

Rev 838 | Rev 981 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 838 Rev 966
Line 13... Line 13...
13 
*/
 13 
*/
Line 14... Line 14...
14 
 
 14 
 
15 
/*****************************************************************************
 15 
/*****************************************************************************
16 
  INCLUDES
 16 
  INCLUDES
17 
**************************************************************************** */
 17 
**************************************************************************** */
Line 18... Line 18...
18 
#include "mat.h"
 18 
#include "matmatrix.h"
19 
 
 19 
 
20 
/*****************************************************************************
 20 
/*****************************************************************************
Line -... Line 21...
- 
 
 21 
  (SYMBOLIC) CONSTANTS
- 
 
 22 
*****************************************************************************/
- 
 
 23 
 
- 
 
 24 
/*****************************************************************************
Line 21... Line 25...
21 
  (SYMBOLIC) CONSTANTS
 25 
  VARIABLES
22 
*****************************************************************************/
 26 
*****************************************************************************/
23 
 
 27 
extern int sollGier;
24 
 
 28 
 
Line 25... Line 29...
25 
/* *************************************************************************** */
 29 
/* *************************************************************************** */
26 
/* Uses the Compass to Estimate Theta, Phi, Psi  */
 30 
/* Uses the Compass to Estimate Theta, Phi, Psi  */
Line 27... Line 31...
27 
//#define USE_Extended_MM3_Measurement_Model
 31 
//#define USE_Extended_MM3_Measurement_Model
28 
/* *************************************************************************** */
 32 
/* *************************************************************************** */
29 
 
 33 
 
30 
/* kalman filter model extensions change number of state variables */
 34 
/* kalman filter model extensions change number of state variables */
31 
#define KAFI_DIM_X   (3)    /* number of rows of state vector */
 35 
#define KAFI_DIM_X   (3)    /* number of rows of state vector */
Line 32... Line 36...
32 
 
 36 
 
Line -... Line 37...
- 
 
 37 
#ifdef USE_Extended_MM3_Measurement_Model
- 
 
 38 
    #define KAFI_DIM_Y   (6)    /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/
- 
 
 39 
#else
33 
#ifdef USE_Extended_MM3_Measurement_Model
 40 
    #define KAFI_DIM_Y   (4)   /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/
34 
        #define KAFI_DIM_Y   (6)    /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/
 41 
#endif
35 
#else
 42 
 
36 
        #define KAFI_DIM_Y   (4)   /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/
 43 
#define KAFI_DIM_U   (3)    /* number of rows of control vector */
- 
 
 44 
 
37 
#endif
 45 
#ifdef MELEXIS_GYRO
Line -... Line 46...
- 
 
 46 
    #define fCycleTime 12.5F; /* Use Different Cycle Times to account for Runtime / Gain Differences */
- 
 
 47 
#else
- 
 
 48 
#ifdef USE_Extended_MM3_Measurement_Model
- 
 
 49 
    #define fCycleTime 16.0F;
- 
 
 50 
#else
- 
 
 51 
    #define fCycleTime 12.5F;
- 
 
 52 
    #endif
- 
 
 53 
#endif
- 
 
 54 
 
- 
 
 55 
/* Predict the Measurent */
- 
 
 56 
void trPredictMeasurement(void);
- 
 
 57 
/* Update the Jacobi Matrix */
- 
 
 58 
void trUpdateJacobiMatrix(void);
- 
 
 59 
/* Extract measurements and store them in vector matY.
- 
 
 60 
   Measurement validity is indicated by fYValid[] */
- 
 
 61 
void trMeasure(void);
- 
 
 62 
/* Innovation: calculate Xd, and Pd  */
- 
 
 63 
void trInnovate(void);
- 
 
 64 
/* Update transition matrix Phi  */
- 
 
 65 
void trUpdatePhi(void);
- 
 
 66 
/* Update control matrix B */
- 
 
 67 
void trUpdateBU(void);
- 
 
 68 
/* Predict new state Xs and Ps */
- 
 
 69 
void KAFIPrediction(void);
- 
 
 70 
/* Main Kalman Filter Loop */
- 
 
 71 
void FlightAttitudeEstimation(void);
- 
 
 72 
/* Setup the Kalman Filter Parameter */
- 
 
 73 
void Kafi_Init(void);
- 
 
 74 
/* Not done yet */
- 
 
 75 
void trEstimateVelocity(void);
- 
 
 76 
/* Limit Angles to [-2Pi;...;+2Pi] */
- 
 
 77 
void trLimitAngles(void);
- 
 
 78 
 
- 
 
 79 
typedef struct
- 
 
 80 
{
- 
 
 81 
f32_t Phi;
- 
 
 82 
f32_t Theta;
- 
 
 83 
f32_t Psi;
- 
 
 84 
i32_t iPhi10;
- 
 
 85 
i32_t iTheta10;
- 
 
 86 
i32_t iPsi10;
- 
 
 87 
f32_t dPhi;
- 
 
 88 
f32_t dTheta;
- 
 
 89 
f32_t dPsi;
- 
 
 90 
f32_t du;
38 
 
 91 
f32_t dv;
39 
#define KAFI_DIM_U   (3)    /* number of rows of control vector */
 92 
f32_t dw;
40 
 
 93 
f32_t X;
41 
#ifdef USE_Extended_MM3_Measurement_Model
 94 
f32_t Y;
42 
        #define fCycleTime 16.0F;
 95 
f32_t Z;
Line 73... Line 126...
73 
  _compass,
 126 
  _compass,
74 
  c_observation_variables                       /* number of observation variables */
 127 
  c_observation_variables                       /* number of observation variables */
75 
} /*trObservationVariables_e*/;
 128 
} /*trObservationVariables_e*/;
76 
#endif
 129 
#endif
Line 77... Line -...
77 
 
 - 
 
78 
 
 130 
 
79 
/*typedef*/ enum
 131 
/*typedef*/ enum
80 
{
 132 
{
81 
   _Phi = 0,
 133 
   _Phi = 0,
82 
   _Theta,
 134 
   _Theta,
Line 86... Line 138...
86 
   //_dw,
 138 
   //_dw,
87 
   c_state_variables                       /* number of state variables */
 139 
   c_state_variables                       /* number of state variables */
88 
} /*trStateVariables_e*/;
 140 
} /*trStateVariables_e*/;
Line 89... Line -...
89 
 
 - 
 
90 
 
 - 
 
91 
typedef struct
 - 
 
92 
{
 - 
 
93 
f32_t Phi;
 - 
 
94 
f32_t Theta;
 - 
 
95 
f32_t Psi;
 - 
 
96 
i32_t iPhi10;
 - 
 
97 
i32_t iTheta10;
 - 
 
98 
i32_t iPsi10;
 - 
 
99 
f32_t dPhi;
 - 
 
100 
f32_t dTheta;
 - 
 
101 
f32_t dPsi;
 - 
 
102 
f32_t du;
 - 
 
103 
f32_t dv;
 - 
 
104 
f32_t dw;
 - 
 
105 
f32_t X;
 - 
 
106 
f32_t Y;
 - 
 
107 
f32_t Z;
 - 
 
108 
} status_t;
 - 
 
109 
 
 - 
 
Line 110... Line -...
110 
status_t status;
 - 
 
111 
 
 - 
 
112 
 
 - 
 
113 
void trUpdatePhi(void);
 - 
 
114 
void trUpdateBU(void);
 - 
 
115 
void trMeasure(void);
 - 
 
116 
void trInnovate(void);
 - 
 
Line 117... Line -...
117 
 
 - 
 
118 
void trResetKalmanFilter(void);
 - 
 
119 
void AttitudeEstimation(void);
 - 
 
120 
 
 - 
 
121 
int  KAFIInit(kafi_t *pkafi);
 -