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