Rev 981 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 981 | Rev 1041 | ||
---|---|---|---|
1 | /* |
1 | /* |
2 | Copyright 2008, by Michael Walter |
2 | Copyright 2008, by Michael Walter |
3 | 3 | ||
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
9 | 9 | ||
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
12 | unless it is stated otherwise. |
12 | unless it is stated otherwise. |
13 | */ |
13 | */ |
14 | 14 | ||
15 | /***************************************************************************** |
15 | /***************************************************************************** |
16 | INCLUDES |
16 | INCLUDES |
17 | **************************************************************************** */ |
17 | **************************************************************************** */ |
18 | #include "matmatrix.h" |
18 | #include "matmatrix.h" |
19 | 19 | ||
20 | /***************************************************************************** |
20 | /***************************************************************************** |
21 | (SYMBOLIC) CONSTANTS |
21 | (SYMBOLIC) CONSTANTS |
22 | *****************************************************************************/ |
22 | *****************************************************************************/ |
23 | 23 | ||
24 | /***************************************************************************** |
24 | /***************************************************************************** |
25 | VARIABLES |
25 | VARIABLES |
26 | *****************************************************************************/ |
26 | *****************************************************************************/ |
27 | extern int sollGier; |
27 | extern int sollGier; |
28 | 28 | ||
29 | /* *************************************************************************** */ |
29 | /* *************************************************************************** */ |
30 | /* Uses the Compass to Estimate Theta, Phi, Psi */ |
30 | /* Uses the Compass to Estimate Theta, Phi, Psi */ |
31 | //#define USE_Extended_MM3_Measurement_Model |
31 | //#define USE_Extended_MM3_Measurement_Model |
32 | /* *************************************************************************** */ |
32 | /* *************************************************************************** */ |
33 | 33 | ||
34 | /* kalman filter model extensions change number of state variables */ |
34 | /* kalman filter model extensions change number of state variables */ |
35 | #define KAFI_DIM_X (3) /* number of rows of state vector */ |
35 | #define KAFI_DIM_X (3) /* number of rows of state vector */ |
36 | 36 | ||
37 | #ifdef USE_Extended_MM3_Measurement_Model |
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)*/ |
38 | #define KAFI_DIM_Y (6) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/ |
39 | #else |
39 | #else |
40 | #define KAFI_DIM_Y (4) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/ |
40 | #define KAFI_DIM_Y (4) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/ |
41 | #endif |
41 | #endif |
42 | 42 | ||
43 | #define KAFI_DIM_U (3) /* number of rows of control vector */ |
43 | #define KAFI_DIM_U (3) /* number of rows of control vector */ |
44 | 44 | ||
45 | #ifdef MELEXIS_GYRO |
45 | #ifdef MELEXIS_GYRO |
46 | #define fCycleTime 12.5F; /* Use Different Cycle Times to account for Runtime / Gain Differences */ |
46 | #define fCycleTime 12.5F /* Use Different Cycle Times to account for Runtime / Gain Differences */ |
47 | #else |
47 | #else |
48 | #ifdef USE_Extended_MM3_Measurement_Model |
48 | #ifdef USE_Extended_MM3_Measurement_Model |
49 | #define fCycleTime 16.0F; |
49 | #define fCycleTime 16.0F |
50 | #else |
50 | #else |
51 | //#define fCycleTime 12.5F; |
51 | //#define fCycleTime 12.5F |
52 | #define fCycleTime 16.0F; |
52 | #define fCycleTime 16.0F |
53 | #endif |
53 | #endif |
54 | #endif |
54 | #endif |
55 | 55 | ||
56 | /* Predict the Measurent */ |
56 | /* Predict the Measurent */ |
57 | void trPredictMeasurement(void); |
57 | void trPredictMeasurement(void); |
58 | /* Update the Jacobi Matrix */ |
58 | /* Update the Jacobi Matrix */ |
59 | void trUpdateJacobiMatrix(void); |
59 | void trUpdateJacobiMatrix(void); |
60 | /* Extract measurements and store them in vector matY. |
60 | /* Extract measurements and store them in vector matY. |
61 | Measurement validity is indicated by fYValid[] */ |
61 | Measurement validity is indicated by fYValid[] */ |
62 | void trMeasure(void); |
62 | void trMeasure(void); |
63 | /* Innovation: calculate Xd, and Pd */ |
63 | /* Innovation: calculate Xd, and Pd */ |
64 | void trInnovate(void); |
64 | void trInnovate(void); |
65 | /* Update transition matrix Phi */ |
65 | /* Update transition matrix Phi */ |
66 | void trUpdatePhi(void); |
66 | void trUpdatePhi(void); |
67 | /* Update control matrix B */ |
67 | /* Update control matrix B */ |
68 | void trUpdateBU(void); |
68 | void trUpdateBU(void); |
69 | /* Predict new state Xs and Ps */ |
69 | /* Predict new state Xs and Ps */ |
70 | void KAFIPrediction(void); |
70 | void KAFIPrediction(void); |
71 | /* Main Kalman Filter Loop */ |
71 | /* Main Kalman Filter Loop */ |
72 | void FlightAttitudeEstimation(void); |
72 | void FlightAttitudeEstimation(void); |
73 | /* Setup the Kalman Filter Parameter */ |
73 | /* Setup the Kalman Filter Parameter */ |
74 | void Kafi_Init(void); |
74 | void Kafi_Init(void); |
75 | /* Not done yet */ |
75 | /* Not done yet */ |
76 | void trEstimateVelocity(void); |
76 | void trEstimateVelocity(void); |
77 | /* Limit Angles to [-2Pi;...;+2Pi] */ |
77 | /* Limit Angles to [-2Pi;...;+2Pi] */ |
78 | void trLimitAngles(void); |
78 | void trLimitAngles(void); |
79 | 79 | ||
80 | typedef struct |
80 | typedef struct |
81 | { |
81 | { |
82 | f32_t Phi; |
82 | f32_t Phi; |
83 | f32_t Theta; |
83 | f32_t Theta; |
84 | f32_t Psi; |
84 | f32_t Psi; |
85 | i32_t iPhi10; |
85 | i32_t iPhi10; |
86 | i32_t iTheta10; |
86 | i32_t iTheta10; |
87 | i32_t iPsi10; |
87 | i32_t iPsi10; |
88 | f32_t dPhi; |
88 | f32_t dPhi; |
89 | f32_t dTheta; |
89 | f32_t dTheta; |
90 | f32_t dPsi; |
90 | f32_t dPsi; |
91 | f32_t du; |
91 | f32_t du; |
92 | f32_t dv; |
92 | f32_t dv; |
93 | f32_t dw; |
93 | f32_t dw; |
94 | f32_t X; |
94 | f32_t X; |
95 | f32_t Y; |
95 | f32_t Y; |
96 | f32_t Z; |
96 | f32_t Z; |
97 | } status_t; |
97 | } status_t; |
98 | 98 | ||
99 | status_t status; |
99 | status_t status; |
100 | 100 | ||
101 | /*typedef*/ enum |
101 | /*typedef*/ enum |
102 | { |
102 | { |
103 | _p = 0, |
103 | _p = 0, |
104 | _q, |
104 | _q, |
105 | _r, |
105 | _r, |
106 | c_control_variables /* number of control variables */ |
106 | c_control_variables /* number of control variables */ |
107 | } /*trControlVariables_e*/; |
107 | } /*trControlVariables_e*/; |
108 | 108 | ||
109 | 109 | ||
110 | #ifdef USE_Extended_MM3_Measurement_Model |
110 | #ifdef USE_Extended_MM3_Measurement_Model |
111 | /*typedef*/ enum |
111 | /*typedef*/ enum |
112 | { |
112 | { |
113 | _ax = 0, |
113 | _ax = 0, |
114 | _ay, |
114 | _ay, |
115 | _az, |
115 | _az, |
116 | _mx, |
116 | _mx, |
117 | _my, |
117 | _my, |
118 | _mz, |
118 | _mz, |
119 | c_observation_variables /* number of observation variables */ |
119 | c_observation_variables /* number of observation variables */ |
120 | } /*trObservationVariables_e*/; |
120 | } /*trObservationVariables_e*/; |
121 | #else |
121 | #else |
122 | /*typedef*/ enum |
122 | /*typedef*/ enum |
123 | { |
123 | { |
124 | _ax = 0, |
124 | _ax = 0, |
125 | _ay, |
125 | _ay, |
126 | _az, |
126 | _az, |
127 | _compass, |
127 | _compass, |
128 | c_observation_variables /* number of observation variables */ |
128 | c_observation_variables /* number of observation variables */ |
129 | } /*trObservationVariables_e*/; |
129 | } /*trObservationVariables_e*/; |
130 | #endif |
130 | #endif |
131 | 131 | ||
132 | /*typedef*/ enum |
132 | /*typedef*/ enum |
133 | { |
133 | { |
134 | _Phi = 0, |
134 | _Phi = 0, |
135 | _Theta, |
135 | _Theta, |
136 | _Psi, |
136 | _Psi, |
137 | //_du, |
137 | //_du, |
138 | //_dv, |
138 | //_dv, |
139 | //_dw, |
139 | //_dw, |
140 | c_state_variables /* number of state variables */ |
140 | c_state_variables /* number of state variables */ |
141 | } /*trStateVariables_e*/; |
141 | } /*trStateVariables_e*/; |
142 | 142 | ||
143 | 143 | ||
144 | 144 | ||
145 | 145 | ||
146 | 146 | ||
147 | 147 | ||
148 | 148 |