Rev 973 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 973 | Rev 982 | ||
---|---|---|---|
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 "kafi.h" |
18 | #include "kafi.h" |
19 | #include "analog.h" |
19 | #include "analog.h" |
20 | #include "main.h" |
20 | #include "main.h" |
21 | #include "FlightControl.h" |
21 | #include "FlightControl.h" |
22 | #include "mm3.h" |
22 | #include "mm3.h" |
23 | 23 | ||
24 | /***************************************************************************** |
24 | /***************************************************************************** |
25 | (SYMBOLIC) CONSTANTS |
25 | (SYMBOLIC) CONSTANTS |
26 | *****************************************************************************/ |
26 | *****************************************************************************/ |
27 | 27 | ||
28 | /***************************************************************************** |
28 | /***************************************************************************** |
29 | VARIABLES |
29 | VARIABLES |
30 | *****************************************************************************/ |
30 | *****************************************************************************/ |
31 | volatile int KompassValue = 0; |
31 | volatile int KompassValue = 0; |
32 | 32 | ||
33 | /* Kalman filter */ |
33 | /* Kalman filter */ |
34 | f32_t Phi, Theta, sinPhi, cosPhi, tanTheta, secTheta; |
34 | f32_t Phi, Theta, sinPhi, cosPhi, tanTheta, secTheta; |
35 | f32_t sinPhi, cosPhi, sinTheta, cosTheta; |
35 | f32_t sinPhi, cosPhi, sinTheta, cosTheta; |
36 | f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P; |
36 | f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P; |
37 | 37 | ||
38 | f32_t **matPs; /* Predicted system Noise */ |
38 | f32_t **matPs; /* Predicted system Noise */ |
39 | f32_t **matP; /* Predicted system Noise */ |
39 | f32_t **matP; /* Predicted system Noise */ |
40 | f32_t **matP_tmp; |
40 | f32_t **matP_tmp; |
41 | f32_t **matQ; /* system noise covariance */ |
41 | f32_t **matQ; /* system noise covariance */ |
42 | f32_t **matR; /* measurement noise covariance */ |
42 | f32_t **matR; /* measurement noise covariance */ |
43 | f32_t **matXd; /* estimated state */ |
43 | f32_t **matXd; /* estimated state */ |
44 | f32_t **matXs; /* predicted state */ |
44 | f32_t **matXs; /* predicted state */ |
45 | f32_t **matX_tmp1; |
45 | f32_t **matX_tmp1; |
46 | f32_t **matX_tmp2; |
46 | f32_t **matX_tmp2; |
47 | 47 | ||
48 | f32_t **matPhi; /* transition matrix to predict new state from current state */ |
48 | f32_t **matPhi; /* transition matrix to predict new state from current state */ |
49 | f32_t **matB ; /* control matrix to predict new state from ext. control vector U */ |
49 | f32_t **matB ; /* control matrix to predict new state from ext. control vector U */ |
50 | f32_t **matU; /* control vector */ |
50 | f32_t **matU; /* control vector */ |
51 | f32_t **matC; /* Jacobi matrix */ |
51 | f32_t **matC; /* Jacobi matrix */ |
52 | f32_t **matY; /* real measurements */ |
52 | f32_t **matY; /* real measurements */ |
53 | f32_t **matYs; /* predicted measurements */ |
53 | f32_t **matYs; /* predicted measurements */ |
54 | f32_t **matdY; /* innovation */ |
54 | f32_t **matdY; /* innovation */ |
55 | f32_t **temp_meas_meas; |
55 | f32_t **temp_meas_meas; |
56 | f32_t **temp_meas_state; |
56 | f32_t **temp_meas_state; |
57 | f32_t **temp_state_meas; |
57 | f32_t **temp_state_meas; |
58 | f32_t **temp_meas_2; |
58 | f32_t **temp_meas_2; |
59 | f32_t **temp_state_state; |
59 | f32_t **temp_state_state; |
60 | f32_t **matK; /* the Kalman gain matrix */ |
60 | f32_t **matK; /* the Kalman gain matrix */ |
61 | 61 | ||
62 | char fYValid[KAFI_DIM_Y]; /* measurement validity flag */ |
62 | char fYValid[KAFI_DIM_Y]; /* measurement validity flag */ |
63 | 63 | ||
64 | 64 | ||
65 | 65 | ||
66 | /* **************************************************************************** |
66 | /* **************************************************************************** |
67 | Functionname: AttitudeEstimation */ /*! |
67 | Functionname: AttitudeEstimation */ /*! |
68 | Description: Main Kalman Filter Loop |
68 | Description: Main Kalman Filter Loop |
69 | |
69 | |
70 | @param[in] |
70 | @param[in] |
71 | 71 | ||
72 | @return void |
72 | @return void |
73 | @pre - |
73 | @pre - |
74 | @post - |
74 | @post - |
75 | @author Michael Walter |
75 | @author Michael Walter |
76 | **************************************************************************** */ |
76 | **************************************************************************** */ |
77 | void FlightAttitudeEstimation() |
77 | void FlightAttitudeEstimation() |
78 | { |
78 | { |
79 | /* Predict the Measurent */ |
79 | /* Predict the Measurent */ |
80 | trPredictMeasurement(); |
80 | trPredictMeasurement(); |
81 | 81 | ||
82 | /* Update the Jacobi Matrix */ |
82 | /* Update the Jacobi Matrix */ |
83 | trUpdateJacobiMatrix(); |
83 | trUpdateJacobiMatrix(); |
84 | 84 | ||
85 | /* Extract measurements and store them in vector matY. |
85 | /* Extract measurements and store them in vector matY. |
86 | Measurement validity is indicated by fYValid[] */ |
86 | Measurement validity is indicated by fYValid[] */ |
87 | trMeasure(); |
87 | trMeasure(); |
88 | 88 | ||
89 | /* Innovation: calculate Xd, and Pd */ |
89 | /* Innovation: calculate Xd, and Pd */ |
90 | trInnovate(); |
90 | trInnovate(); |
91 | 91 | ||
92 | /* Update transition matrix Phi */ |
92 | /* Update transition matrix Phi */ |
93 | //trUpdatePhi(); // Is an Identity matrix which will not change |
93 | //trUpdatePhi(); // Is an Identity matrix which will not change |
94 | 94 | ||
95 | /* Update control matrix B */ |
95 | /* Update control matrix B */ |
96 | trUpdateBU(); |
96 | trUpdateBU(); |
97 | 97 | ||
98 | /* Predict new state Xs and Ps */ |
98 | /* Predict new state Xs and Ps */ |
99 | KAFIPrediction(); |
99 | KAFIPrediction(); |
100 | 100 | ||
101 | /* store innovated state vector in current state */ |
101 | /* store innovated state vector in current state */ |
102 | matGetFull(matXs, _Phi , 0, &status.Phi); |
102 | matGetFull(matXs, _Phi , 0, &status.Phi); |
103 | matGetFull(matXs, _Theta , 0, &status.Theta); |
103 | matGetFull(matXs, _Theta , 0, &status.Theta); |
104 | matGetFull(matXs, _Psi , 0, &status.Psi); |
104 | matGetFull(matXs, _Psi , 0, &status.Psi); |
105 | 105 | ||
106 | /* Limit Angles to [-2Pi;...;+2Pi] */ |
106 | /* Limit Angles to [-2Pi;...;+2Pi] */ |
107 | trLimitAngles(); |
107 | trLimitAngles(); |
108 | } |
108 | } |
109 | 109 | ||
110 | /* **************************************************************************** |
110 | /* **************************************************************************** |
111 | Functionname: Kafi_Init */ /*! |
111 | Functionname: Kafi_Init */ /*! |
112 | Description: Generate Kalman Matrizes |
112 | Description: Generate Kalman Matrizes |
113 | |
113 | |
114 | @param[in] |
114 | @param[in] |
115 | 115 | ||
116 | @return void |
116 | @return void |
117 | @pre - |
117 | @pre - |
118 | @post - |
118 | @post - |
119 | @author Michael Walter |
119 | @author Michael Walter |
120 | **************************************************************************** */ |
120 | **************************************************************************** */ |
121 | void Kafi_Init() |
121 | void Kafi_Init() |
122 | { |
122 | { |
123 | /* create kalman filter and associated matrizes */ |
123 | /* create kalman filter and associated matrizes */ |
124 | ui32_t i, j; |
124 | ui32_t i, j; |
125 | matPs = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
125 | matPs = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
126 | matQ = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
126 | matQ = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
127 | matR = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_Y); |
127 | matR = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_Y); |
128 | matXd = matrix( 1, KAFI_DIM_X, 1,1); /* estimated state */ |
128 | matXd = matrix( 1, KAFI_DIM_X, 1,1); /* estimated state */ |
129 | matXs = matrix( 1, KAFI_DIM_X, 1,1); /* predicted state */ |
129 | matXs = matrix( 1, KAFI_DIM_X, 1,1); /* predicted state */ |
130 | matX_tmp1 = matrix( 1, KAFI_DIM_X,1, 1); |
130 | matX_tmp1 = matrix( 1, KAFI_DIM_X,1, 1); |
131 | matX_tmp2 = matrix( 1, KAFI_DIM_X, 1,1); |
131 | matX_tmp2 = matrix( 1, KAFI_DIM_X, 1,1); |
132 | matPhi = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);/* transition matrix to predict new state from current state */ |
132 | matPhi = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);/* transition matrix to predict new state from current state */ |
133 | matB = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_U); /* control matrix to predict new state from ext. control vector U */ |
133 | matB = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_U); /* control matrix to predict new state from ext. control vector U */ |
134 | matU = matrix( 1, KAFI_DIM_U, 1,1); /* control vector */ |
134 | matU = matrix( 1, KAFI_DIM_U, 1,1); /* control vector */ |
135 | matC = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_X); /* Jacobi matrix */ |
135 | matC = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_X); /* Jacobi matrix */ |
136 | matY = matrix( 1, KAFI_DIM_Y, 1,1); /* real measurements */ |
136 | matY = matrix( 1, KAFI_DIM_Y, 1,1); /* real measurements */ |
137 | matdY = matrix( 1, KAFI_DIM_Y, 1,1); /* innovation */ |
137 | matdY = matrix( 1, KAFI_DIM_Y, 1,1); /* innovation */ |
138 | matYs = matrix( 1, KAFI_DIM_Y, 1,1); /* predicted measurements */ |
138 | matYs = matrix( 1, KAFI_DIM_Y, 1,1); /* predicted measurements */ |
139 | matP = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
139 | matP = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
140 | matK = matrix(1, KAFI_DIM_X, 1, KAFI_DIM_Y); |
140 | matK = matrix(1, KAFI_DIM_X, 1, KAFI_DIM_Y); |
141 | temp_state_state = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
141 | temp_state_state = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
142 | temp_meas_state = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_X ); |
142 | temp_meas_state = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_X ); |
143 | temp_state_meas = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_Y ); |
143 | temp_state_meas = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_Y ); |
144 | temp_meas_meas = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
144 | temp_meas_meas = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
145 | temp_meas_2 = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
145 | temp_meas_2 = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
146 | matP_tmp = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X); |
146 | matP_tmp = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X); |
147 | 147 | ||
148 | /* set phi to identity matrix */ |
148 | /* set phi to identity matrix */ |
149 | for ( j = 0; j < KAFI_DIM_X; j++ ) |
149 | for ( j = 0; j < KAFI_DIM_X; j++ ) |
150 | { |
150 | { |
151 | for ( i = 0; i < KAFI_DIM_X; i++ ) |
151 | for ( i = 0; i < KAFI_DIM_X; i++ ) |
152 | { |
152 | { |
153 | matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F); |
153 | matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F); |
154 | } |
154 | } |
155 | } |
155 | } |
156 | 156 | ||
157 | /* set diagonal of measurement noise covariance R */ |
157 | /* set diagonal of measurement noise covariance R */ |
158 | matSetDiagonal(matR, _ax, _ax, 10.0F); |
158 | matSetDiagonal(matR, _ax, _ax, 10.0F); |
159 | matSetDiagonal(matR, _ay, _ay, 10.0F); |
159 | matSetDiagonal(matR, _ay, _ay, 10.0F); |
160 | matSetDiagonal(matR, _az, _az, 10.0F); |
160 | matSetDiagonal(matR, _az, _az, 10.0F); |
161 | matSetDiagonal(matR, _compass, _compass, 5.0F); |
161 | matSetDiagonal(matR, _compass, _compass, 5.0F); |
162 | 162 | ||
163 | #ifdef MELEXIS_GYRO |
163 | #ifdef MELEXIS_GYRO |
164 | /* set diagonal of system noise covariance Q, dim(X,X) */ |
164 | /* set diagonal of system noise covariance Q, dim(X,X) */ |
165 | matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
165 | matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
166 | matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
166 | matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
167 | matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
167 | matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
168 | #else |
168 | #else |
169 | /* set diagonal of system noise covariance Q */ |
169 | /* set diagonal of system noise covariance Q */ |
170 | matSetDiagonal(matQ, _Phi, _Phi, 0.00001F); |
170 | matSetDiagonal(matQ, _Phi, _Phi, 0.00001F); |
171 | matSetDiagonal(matQ, _Theta, _Theta, 0.00001F); |
171 | matSetDiagonal(matQ, _Theta, _Theta, 0.00001F); |
172 | matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
172 | matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
173 | #endif |
173 | #endif |
- | 174 | ||
- | 175 | matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
|
- | 176 | matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
|
- | 177 | matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
|
174 | 178 | ||
175 | /* set diagonal init. estimation error (covariance) P0 */ |
179 | /* set diagonal init. estimation error (covariance) P0 */ |
176 | matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
180 | matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
177 | matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
181 | matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
178 | matSetDiagonal(matPs, _Psi, _Psi, 0.00001F); |
182 | matSetDiagonal(matPs, _Psi, _Psi, 0.00001F); |
179 | 183 | ||
180 | matSetFull(matXd, _Phi, 0, 0.0F); |
184 | matSetFull(matXd, _Phi, 0, 0.0F); |
181 | matSetFull(matXd, _Theta, 0, 0.0F); |
185 | matSetFull(matXd, _Theta, 0, 0.0F); |
182 | matSetFull(matXd, _Psi, 0, 0.0F); |
186 | matSetFull(matXd, _Psi, 0, 0.0F); |
183 | 187 | ||
184 | matSetFull(matXs, _Phi, 0, 0.0F); |
188 | matSetFull(matXs, _Phi, 0, 0.0F); |
185 | matSetFull(matXs, _Theta, 0, 0.0F); |
189 | matSetFull(matXs, _Theta, 0, 0.0F); |
186 | matSetFull(matXs, _Psi, 0, 0.0F); |
190 | matSetFull(matXs, _Psi, 0, 0.0F); |
187 | } |
191 | } |
188 | 192 | ||
189 | /* **************************************************************************** |
193 | /* **************************************************************************** |
190 | Functionname: KAFIPrediction */ /*! |
194 | Functionname: KAFIPrediction */ /*! |
191 | Description: Predict new state Xs and Ps |
195 | Description: Predict new state Xs and Ps |
192 | |
196 | |
193 | X_k = A * X_(k-1) + B * u_k |
197 | X_k = A * X_(k-1) + B * u_k |
194 | P_k = A * P_(k-1) * A^T + Q |
198 | P_k = A * P_(k-1) * A^T + Q |
195 | 199 | ||
196 | @param[in] |
200 | @param[in] |
197 | 201 | ||
198 | @return void |
202 | @return void |
199 | @pre - |
203 | @pre - |
200 | @post - |
204 | @post - |
201 | @author Michael Walter |
205 | @author Michael Walter |
202 | **************************************************************************** */ |
206 | **************************************************************************** */ |
203 | void KAFIPrediction() |
207 | void KAFIPrediction() |
204 | { |
208 | { |
205 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
209 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
206 | 210 | ||
207 | /*--- VARIABLES ---*/ |
211 | /*--- VARIABLES ---*/ |
208 | 212 | ||
209 | /* Innovation: calculate Xd, and Pd */ |
213 | /* Innovation: calculate Xd, and Pd */ |
210 | 214 | ||
211 | /* X_k = A * X_(k-1) + B * u_k */ |
215 | /* X_k = A * X_(k-1) + B * u_k */ |
212 | 216 | ||
213 | /* matX_tmp1 = matPhi * matXd */ |
217 | /* matX_tmp1 = matPhi * matXd */ |
214 | /* |
218 | /* |
215 | matrix_mult(matPhi,matXd, matX_tmp1, |
219 | matrix_mult(matPhi,matXd, matX_tmp1, |
216 | KAFI_DIM_X, KAFI_DIM_X, 1); |
220 | KAFI_DIM_X, KAFI_DIM_X, 1); |
217 | */ |
221 | */ |
218 | /* matX_tmp2 = matB * matU */ |
222 | /* matX_tmp2 = matB * matU */ |
219 | matrix_mult( matB,matU, matX_tmp2, |
223 | matrix_mult( matB,matU, matX_tmp2, |
220 | KAFI_DIM_X, KAFI_DIM_U, 1); |
224 | KAFI_DIM_X, KAFI_DIM_U, 1); |
221 | 225 | ||
222 | /* matXs = matPhi * matXd + matB * matU */ |
226 | /* matXs = matPhi * matXd + matB * matU */ |
223 | /* |
227 | /* |
224 | matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
228 | matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
225 | matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
229 | matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
226 | */ |
230 | */ |
227 | 231 | ||
228 | matXs[1][1] = matXd[1][1] + matX_tmp2[1][1]; |
232 | matXs[1][1] = matXd[1][1] + matX_tmp2[1][1]; |
229 | matXs[2][1] = matXd[2][1] + matX_tmp2[2][1]; |
233 | matXs[2][1] = matXd[2][1] + matX_tmp2[2][1]; |
230 | matXs[3][1] = matXd[3][1] + matX_tmp2[3][1]; |
234 | matXs[3][1] = matXd[3][1] + matX_tmp2[3][1]; |
231 | 235 | ||
232 | /*P_k = A * P_(k-1)*A^T + Q */ |
236 | /*P_k = A * P_(k-1)*A^T + Q */ |
233 | /* |
237 | /* |
234 | matrix_mult_transpose( matP, matPhi, matP_tmp, // Can be skiped PHI is a ident matrix |
238 | matrix_mult_transpose( matP, matPhi, matP_tmp, // Can be skiped PHI is a ident matrix |
235 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
239 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
236 | matrix_mult( matPhi, matP_tmp, matP, |
240 | matrix_mult( matPhi, matP_tmp, matP, |
237 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
241 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
238 | matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X ); |
242 | matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X ); |
239 | */ |
243 | */ |
240 | 244 | ||
241 | matPs[1][1] = matP[1][1] + matQ[1][1]; |
245 | matPs[1][1] = matP[1][1] + matQ[1][1]; |
242 | matPs[2][2] = matP[2][2] + matQ[2][2]; |
246 | matPs[2][2] = matP[2][2] + matQ[2][2]; |
243 | matPs[3][3] = matP[3][3] + matQ[3][3]; |
247 | matPs[3][3] = matP[3][3] + matQ[3][3]; |
244 | } |
248 | } |
245 | 249 | ||
246 | /* **************************************************************************** |
250 | /* **************************************************************************** |
247 | Functionname: trInnovate */ /*! |
251 | Functionname: trInnovate */ /*! |
248 | Description: Innovation: calculate Xd, and Pd |
252 | Description: Innovation: calculate Xd, and Pd |
249 | |
253 | |
250 | K = Ps * C^T * (C * Ps * C^T + R)^-1 |
254 | K = Ps * C^T * (C * Ps * C^T + R)^-1 |
251 | X = Xs + K (Y - Ys) |
255 | X = Xs + K (Y - Ys) |
252 | |
256 | |
253 | @param[in] |
257 | @param[in] |
254 | 258 | ||
255 | @return void |
259 | @return void |
256 | @pre - |
260 | @pre - |
257 | @post - |
261 | @post - |
258 | @author Michael Walter |
262 | @author Michael Walter |
259 | **************************************************************************** */ |
263 | **************************************************************************** */ |
260 | void trInnovate() |
264 | void trInnovate() |
261 | { |
265 | { |
262 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
266 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
263 | 267 | ||
264 | /*--- VARIABLES ---*/ |
268 | /*--- VARIABLES ---*/ |
265 | #if 0 /* Runtime optimised. To be verified */ |
269 | #if 0 /* Runtime optimised. To be verified */ |
266 | /* temp_meas_state = matC * matPs */ |
270 | /* temp_meas_state = matC * matPs */ |
267 | matrix_mult( matC, matPs, temp_meas_state, |
271 | matrix_mult( matC, matPs, temp_meas_state, |
268 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
272 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
269 | 273 | ||
270 | /* temp_meas_meas = matC * matPs^T * matC^T */ |
274 | /* temp_meas_meas = matC * matPs^T * matC^T */ |
271 | matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas, |
275 | matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas, |
272 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
276 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
273 | 277 | ||
274 | /* temp_meas_meas = matC * matPs^T * matC^T + matR */ |
278 | /* temp_meas_meas = matC * matPs^T * matC^T + matR */ |
275 | /* |
279 | /* |
276 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
280 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
277 | KAFI_DIM_Y, KAFI_DIM_Y ); |
281 | KAFI_DIM_Y, KAFI_DIM_Y ); |
278 | */ |
282 | */ |
279 | temp_meas_meas[1][1] += matR[1][1]; |
283 | temp_meas_meas[1][1] += matR[1][1]; |
280 | temp_meas_meas[2][2] += matR[2][2]; |
284 | temp_meas_meas[2][2] += matR[2][2]; |
281 | temp_meas_meas[3][3] += matR[3][3]; |
285 | temp_meas_meas[3][3] += matR[3][3]; |
282 | temp_meas_meas[4][4] += matR[4][4]; |
286 | temp_meas_meas[4][4] += matR[4][4]; |
283 | 287 | ||
284 | /* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */ |
288 | /* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */ |
285 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
289 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
286 | 290 | ||
287 | /* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
291 | /* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
288 | matrix_transpose_mult( temp_meas_state, temp_meas_2, matK, |
292 | matrix_transpose_mult( temp_meas_state, temp_meas_2, matK, |
289 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
293 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
290 | 294 | ||
291 | /* temp_state_state = matK * matC * matPs */ |
295 | /* temp_state_state = matK * matC * matPs */ |
292 | matrix_mult( matK, temp_meas_state, temp_state_state, |
296 | matrix_mult( matK, temp_meas_state, temp_state_state, |
293 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
297 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
294 | 298 | ||
295 | /* matP = matPs - matK * matC * matPs */ |
299 | /* matP = matPs - matK * matC * matPs */ |
296 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
300 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
297 | #else |
301 | #else |
298 | /* temp_state_meas = matPs * matC^T */ |
302 | /* temp_state_meas = matPs * matC^T */ |
299 | matrix_mult_transpose( matPs, matC, temp_state_meas, |
303 | matrix_mult_transpose( matPs, matC, temp_state_meas, |
300 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y ); |
304 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y ); |
301 | 305 | ||
302 | /* temp_meas_meas = matC * matPs * matC^T */ |
306 | /* temp_meas_meas = matC * matPs * matC^T */ |
303 | matrix_mult( matC, temp_state_meas, temp_meas_meas, |
307 | matrix_mult( matC, temp_state_meas, temp_meas_meas, |
304 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
308 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
305 | 309 | ||
306 | /* temp_meas_meas = matC * matPs * matC^T + matR */ |
310 | /* temp_meas_meas = matC * matPs * matC^T + matR */ |
307 | /* |
311 | /* |
308 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
312 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
309 | KAFI_DIM_Y, KAFI_DIM_Y ); |
313 | KAFI_DIM_Y, KAFI_DIM_Y ); |
310 | */ |
314 | */ |
311 | temp_meas_meas[1][1] += matR[1][1]; |
315 | temp_meas_meas[1][1] += matR[1][1]; |
312 | temp_meas_meas[2][2] += matR[2][2]; |
316 | temp_meas_meas[2][2] += matR[2][2]; |
313 | temp_meas_meas[3][3] += matR[3][3]; |
317 | temp_meas_meas[3][3] += matR[3][3]; |
314 | temp_meas_meas[4][4] += matR[4][4]; |
318 | temp_meas_meas[4][4] += matR[4][4]; |
315 | 319 | ||
316 | /* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */ |
320 | /* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */ |
317 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
321 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
318 | 322 | ||
319 | /* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
323 | /* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
320 | matrix_mult( temp_state_meas, temp_meas_2, matK, |
324 | matrix_mult( temp_state_meas, temp_meas_2, matK, |
321 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
325 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
322 | 326 | ||
323 | /* temp_meas_state = matC * matPs*/ |
327 | /* temp_meas_state = matC * matPs*/ |
324 | matrix_mult( matC, matPs, temp_meas_state, |
328 | matrix_mult( matC, matPs, temp_meas_state, |
325 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
329 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
326 | 330 | ||
327 | /* temp_state_state = matK * matC * matPs */ |
331 | /* temp_state_state = matK * matC * matPs */ |
328 | matrix_mult( matK, temp_meas_state, temp_state_state, |
332 | matrix_mult( matK, temp_meas_state, temp_state_state, |
329 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
333 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
330 | 334 | ||
331 | /* matP = matPs - matK * matC * matPs */ |
335 | /* matP = matPs - matK * matC * matPs */ |
332 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
336 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
333 | #endif |
337 | #endif |
334 | 338 | ||
335 | /* matdY = matY - matYs */ |
339 | /* matdY = matY - matYs */ |
336 | /* |
340 | /* |
337 | matrix_sub( matY, matYs, matdY, |
341 | matrix_sub( matY, matYs, matdY, |
338 | KAFI_DIM_Y, 1 ); |
342 | KAFI_DIM_Y, 1 ); |
339 | */ |
343 | */ |
340 | matdY[1][1] = matY[1][1] - matYs[1][1]; |
344 | matdY[1][1] = matY[1][1] - matYs[1][1]; |
341 | matdY[2][1] = matY[2][1] - matYs[2][1]; |
345 | matdY[2][1] = matY[2][1] - matYs[2][1]; |
342 | matdY[3][1] = matY[3][1] - matYs[3][1]; |
346 | matdY[3][1] = matY[3][1] - matYs[3][1]; |
343 | 347 | ||
344 | /* We will only update the Heading if we got a valid compass signal */ |
348 | /* We will only update the Heading if we got a valid compass signal */ |
345 | if(fYValid[_compass] == 1) |
349 | if(fYValid[_compass] == 1) |
346 | { |
350 | { |
347 | matdY[4][1] = matY[4][1] - matYs[4][1]; |
351 | matdY[4][1] = matY[4][1] - matYs[4][1]; |
348 | } |
352 | } |
349 | else |
353 | else |
350 | { |
354 | { |
351 | matdY[4][1] = 0.0F; |
355 | matdY[4][1] = 0.0F; |
352 | } |
356 | } |
353 | 357 | ||
354 | /* matX_tmp1 = matK * (Y -Ys) */ |
358 | /* matX_tmp1 = matK * (Y -Ys) */ |
355 | matrix_mult( matK, matdY, matX_tmp1, |
359 | matrix_mult( matK, matdY, matX_tmp1, |
356 | KAFI_DIM_X, KAFI_DIM_Y, 1 ); |
360 | KAFI_DIM_X, KAFI_DIM_Y, 1 ); |
357 | 361 | ||
358 | /* matXd = matXs + matK * (Y -Ys) */ |
362 | /* matXd = matXs + matK * (Y -Ys) */ |
359 | /* |
363 | /* |
360 | matrix_add( matXs, matX_tmp1, matXd, |
364 | matrix_add( matXs, matX_tmp1, matXd, |
361 | KAFI_DIM_X, 1 ); |
365 | KAFI_DIM_X, 1 ); |
362 | */ |
366 | */ |
363 | matXd[1][1] = matXs[1][1] + matX_tmp1[1][1]; |
367 | matXd[1][1] = matXs[1][1] + matX_tmp1[1][1]; |
364 | matXd[2][1] = matXs[2][1] + matX_tmp1[2][1]; |
368 | matXd[2][1] = matXs[2][1] + matX_tmp1[2][1]; |
365 | matXd[3][1] = matXs[3][1] + matX_tmp1[3][1]; |
369 | matXd[3][1] = matXs[3][1] + matX_tmp1[3][1]; |
366 | matXd[4][1] = matXs[4][1] + matX_tmp1[4][1]; |
370 | matXd[4][1] = matXs[4][1] + matX_tmp1[4][1]; |
367 | } |
371 | } |
368 | 372 | ||
369 | 373 | ||
370 | /* **************************************************************************** |
374 | /* **************************************************************************** |
371 | Functionname: trUpdatePhi */ /*! |
375 | Functionname: trUpdatePhi */ /*! |
372 | Description: Setup matPhi, used to calculate the predicted state |
376 | Description: Setup matPhi, used to calculate the predicted state |
373 | from the current state |
377 | from the current state |
374 | |
378 | |
375 | @param[in] |
379 | @param[in] |
376 | @param[in] fCycleTime |
380 | @param[in] fCycleTime |
377 | 381 | ||
378 | @return void |
382 | @return void |
379 | @pre - |
383 | @pre - |
380 | @post - |
384 | @post - |
381 | @author Michael Walter |
385 | @author Michael Walter |
382 | **************************************************************************** */ |
386 | **************************************************************************** */ |
383 | void trUpdatePhi() |
387 | void trUpdatePhi() |
384 | { |
388 | { |
385 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
389 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
386 | 390 | ||
387 | /*--- VARIABLES ---*/ |
391 | /*--- VARIABLES ---*/ |
388 | matSetFull(matPhi, _Phi, _Phi, 1.0F); |
392 | matSetFull(matPhi, _Phi, _Phi, 1.0F); |
389 | matSetFull(matPhi, _Theta, _Theta, 1.0F); |
393 | matSetFull(matPhi, _Theta, _Theta, 1.0F); |
390 | matSetFull(matPhi, _Psi, _Psi, 1.0F); |
394 | matSetFull(matPhi, _Psi, _Psi, 1.0F); |
391 | } |
395 | } |
392 | 396 | ||
393 | /* **************************************************************************** |
397 | /* **************************************************************************** |
394 | Functionname: trUpdateJacobiMatrix */ /*! |
398 | Functionname: trUpdateJacobiMatrix */ /*! |
395 | Description: |
399 | Description: |
396 | |
400 | |
397 | @param[in] void |
401 | @param[in] void |
398 | 402 | ||
399 | @return void |
403 | @return void |
400 | @pre - |
404 | @pre - |
401 | @post - |
405 | @post - |
402 | @author Michael Walter |
406 | @author Michael Walter |
403 | *****************************************************************************/ |
407 | *****************************************************************************/ |
404 | void trUpdateJacobiMatrix(void) |
408 | void trUpdateJacobiMatrix(void) |
405 | { |
409 | { |
406 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
410 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
407 | /* |
411 | /* |
408 | Simplified Version d/dt |
412 | Simplified Version d/dt |
409 | Pre_ax = du - sinTheta * g; |
413 | Pre_ax = du - sinTheta * g; |
410 | Pre_ay = dv + cosTheta_sinPhi * g; |
414 | Pre_ay = dv + cosTheta_sinPhi * g; |
411 | Pre_az = dw + cosTheta_cosPhi * g; |
415 | Pre_az = dw + cosTheta_cosPhi * g; |
412 | */ |
416 | */ |
413 | 417 | ||
414 | /*--- VARIABLES ---*/ |
418 | /*--- VARIABLES ---*/ |
415 | f32_t Phi, Theta, Psi; |
419 | f32_t Phi, Theta, Psi; |
416 | f32_t g = 9.81F; |
420 | f32_t g = 9.81F; |
417 | 421 | ||
418 | Phi = status.Phi; |
422 | Phi = status.Phi; |
419 | Theta = status.Theta; |
423 | Theta = status.Theta; |
420 | Psi = status.Psi; |
424 | Psi = status.Psi; |
421 | 425 | ||
422 | sinPhi = sin(Phi); |
426 | sinPhi = sin(Phi); |
423 | cosPhi = cos(Phi); |
427 | cosPhi = cos(Phi); |
424 | sinTheta = sin(Theta); |
428 | sinTheta = sin(Theta); |
425 | cosTheta = cos(Theta); |
429 | cosTheta = cos(Theta); |
426 | 430 | ||
427 | //Pre_ax = du - sinTheta * g; |
431 | //Pre_ax = du - sinTheta * g; |
428 | //matSetFull(matC, _ax, _Phi , 0); |
432 | //matSetFull(matC, _ax, _Phi , 0); |
429 | matSetFull(matC, _ax, _Theta, -cosTheta * g); |
433 | matSetFull(matC, _ax, _Theta, -cosTheta * g); |
430 | //matSetFull(matC, _ax, _Psi , 0); |
434 | //matSetFull(matC, _ax, _Psi , 0); |
431 | 435 | ||
432 | //Pre_ay = dv + cosTheta_sinPhi * g; |
436 | //Pre_ay = dv + cosTheta_sinPhi * g; |
433 | matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
437 | matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
434 | matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
438 | matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
435 | //matSetFull(matC, _ay, _Psi , 0); |
439 | //matSetFull(matC, _ay, _Psi , 0); |
436 | 440 | ||
437 | //Pre_az = dw + cosTheta_cosPhi * g; |
441 | //Pre_az = dw + cosTheta_cosPhi * g; |
438 | matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
442 | matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
439 | matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
443 | matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
440 | //matSetFull(matC, _az, _Psi , 0); |
444 | //matSetFull(matC, _az, _Psi , 0); |
441 | 445 | ||
442 | matSetFull(matC, _compass, _Psi , 1.0F); |
446 | matSetFull(matC, _compass, _Psi , 1.0F); |
443 | } |
447 | } |
444 | 448 | ||
445 | /***************************************************************************** |
449 | /***************************************************************************** |
446 | Functionname: trPredictMeasurement */ /*! |
450 | Functionname: trPredictMeasurement */ /*! |
447 | Description:Predict Measurements: AX, AY, AZ, Compass |
451 | Description:Predict Measurements: AX, AY, AZ, Compass |
448 | 452 | ||
449 | |
453 | |
450 | @param[in] void |
454 | @param[in] void |
451 | 455 | ||
452 | @return void |
456 | @return void |
453 | @pre - |
457 | @pre - |
454 | @post - |
458 | @post - |
455 | @author Michael Walter |
459 | @author Michael Walter |
456 | *****************************************************************************/ |
460 | *****************************************************************************/ |
457 | void trPredictMeasurement(void) |
461 | void trPredictMeasurement(void) |
458 | { |
462 | { |
459 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
463 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
460 | 464 | ||
461 | /* |
465 | /* |
462 | Simplified Version |
466 | Simplified Version |
463 | Pre_ax = du - sinTheta * g; |
467 | Pre_ax = du - sinTheta * g; |
464 | Pre_ay = dv + cosTheta_sinPhi * g; |
468 | Pre_ay = dv + cosTheta_sinPhi * g; |
465 | Pre_az = dw + cosTheta_cosPhi * g; |
469 | Pre_az = dw + cosTheta_cosPhi * g; |
466 | */ |
470 | */ |
467 | 471 | ||
468 | /*--- VARIABLES ---*/ |
472 | /*--- VARIABLES ---*/ |
469 | f32_t g = 9.81F; |
473 | f32_t g = 9.81F; |
470 | f32_t Pre_ax, Pre_ay, Pre_az; |
474 | f32_t Pre_ax, Pre_ay, Pre_az; |
471 | f32_t Phi, Theta, Psi; |
475 | f32_t Phi, Theta, Psi; |
472 | 476 | ||
473 | matGetFull(matXs, _Phi, 0, &Phi); |
477 | matGetFull(matXs, _Phi, 0, &Phi); |
474 | matGetFull(matXs, _Theta, 0, &Theta); |
478 | matGetFull(matXs, _Theta, 0, &Theta); |
475 | matGetFull(matXs, _Psi, 0, &Psi); |
479 | matGetFull(matXs, _Psi, 0, &Psi); |
476 | 480 | ||
477 | sinPhi_P = sin(Phi); |
481 | sinPhi_P = sin(Phi); |
478 | cosPhi_P = cos(Phi); |
482 | cosPhi_P = cos(Phi); |
479 | sinTheta_P = sin(Theta); |
483 | sinTheta_P = sin(Theta); |
480 | cosTheta_P = cos(Theta); |
484 | cosTheta_P = cos(Theta); |
481 | 485 | ||
482 | /* Simplified Version */ |
486 | /* Simplified Version */ |
483 | Pre_ax = -sinTheta_P * g; |
487 | Pre_ax = -sinTheta_P * g; |
484 | Pre_ay = cosTheta_P * sinPhi_P * g; |
488 | Pre_ay = cosTheta_P * sinPhi_P * g; |
485 | Pre_az = cosTheta_P * cosPhi_P * g; |
489 | Pre_az = cosTheta_P * cosPhi_P * g; |
486 | 490 | ||
487 | matSetFull(matYs, _ax, 0, Pre_ax); |
491 | matSetFull(matYs, _ax, 0, Pre_ax); |
488 | matSetFull(matYs, _ay, 0, Pre_ay); |
492 | matSetFull(matYs, _ay, 0, Pre_ay); |
489 | matSetFull(matYs, _az, 0, Pre_az); |
493 | matSetFull(matYs, _az, 0, Pre_az); |
490 | matSetFull(matYs, _compass, 0, Psi); |
494 | matSetFull(matYs, _compass, 0, Psi); |
491 | } |
495 | } |
492 | 496 | ||
493 | /* **************************************************************************** |
497 | /* **************************************************************************** |
494 | Functionname: trMeasure */ /*! |
498 | Functionname: trMeasure */ /*! |
495 | Description: |
499 | Description: |
496 | 500 | ||
497 | @param[in] |
501 | @param[in] |
498 | |
502 | |
499 | @return void |
503 | @return void |
500 | @pre - |
504 | @pre - |
501 | @post - |
505 | @post - |
502 | @author Michael Walter |
506 | @author Michael Walter |
503 | **************************************************************************** */ |
507 | **************************************************************************** */ |
504 | void trMeasure() |
508 | void trMeasure() |
505 | { |
509 | { |
506 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
510 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
507 | 511 | ||
508 | /*--- VARIABLES ---*/ |
512 | /*--- VARIABLES ---*/ |
509 | #ifdef USE_COMPASS |
513 | #ifdef USE_COMPASS |
510 | static int updCompass = 10; |
514 | static int updCompass = 10; |
511 | f32_t Psi, Compass; |
515 | f32_t Psi, Compass; |
512 | #endif |
516 | #endif |
513 | 517 | ||
514 | f32_t ADC_ax = 0.0F; |
518 | f32_t ADC_ax = 0.0F; |
515 | f32_t ADC_ay = 0.0F; |
519 | f32_t ADC_ay = 0.0F; |
516 | f32_t ADC_az = 0.0F; |
520 | f32_t ADC_az = 0.0F; |
517 | 521 | ||
518 | fYValid[_ax] = 1; |
522 | fYValid[_ax] = 1; |
519 | fYValid[_ay] = 1; |
523 | fYValid[_ay] = 1; |
520 | fYValid[_az] = 1; |
524 | fYValid[_az] = 1; |
521 | 525 | ||
522 | #ifdef INTERNAL_REFERENCE |
526 | #ifdef INTERNAL_REFERENCE |
523 | ADC_ax = AdWertAccNick * 0.044F; |
527 | ADC_ax = AdWertAccNick * 0.044F; |
524 | ADC_ay = -AdWertAccRoll * 0.044F; |
528 | ADC_ay = -AdWertAccRoll * 0.044F; |
525 | ADC_az = AdWertAccHoch * 0.044F; |
529 | ADC_az = AdWertAccHoch * 0.044F; |
526 | #else |
530 | #else |
527 | ADC_ax = AdWertAccNick * 0.047F; |
531 | ADC_ax = AdWertAccNick * 0.047F; |
528 | ADC_ay = -AdWertAccRoll * 0.047F; |
532 | ADC_ay = -AdWertAccRoll * 0.047F; |
529 | ADC_az = AdWertAccHoch * 0.047F; |
533 | ADC_az = AdWertAccHoch * 0.047F; |
530 | #endif |
534 | #endif |
531 | 535 | ||
532 | matSetFull(matY, _ax, 0, ADC_ax); |
536 | matSetFull(matY, _ax, 0, ADC_ax); |
533 | matSetFull(matY, _ay, 0, ADC_ay); |
537 | matSetFull(matY, _ay, 0, ADC_ay); |
534 | matSetFull(matY, _az, 0, ADC_az); |
538 | matSetFull(matY, _az, 0, ADC_az); |
535 | 539 | ||
536 | #ifdef USE_COMPASS |
540 | #ifdef USE_COMPASS |
537 | if (!updCompass--) |
541 | if (!updCompass--) |
538 | { |
542 | { |
539 | updCompass = 10; // update only at 2ms*50 = 100ms (10Hz) |
543 | updCompass = 10; // update only at 2ms*50 = 100ms (10Hz) |
540 | KompassValue = MM3_Heading(); |
544 | KompassValue = MM3_Heading(); |
541 | } |
545 | } |
542 | Compass = KompassValue / 180.F * PI; |
546 | Compass = KompassValue / 180.F * PI; |
543 | 547 | ||
544 | matGetFull(matXs, _Psi, 0, &Psi); |
548 | matGetFull(matXs, _Psi, 0, &Psi); |
545 | 549 | ||
546 | if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi)) |
550 | if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi)) |
547 | { |
551 | { |
548 | Compass += 2 * PI; |
552 | Compass += 2 * PI; |
549 | } |
553 | } |
550 | else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi)) |
554 | else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi)) |
551 | { |
555 | { |
552 | Compass -= 2 * PI; |
556 | Compass -= 2 * PI; |
553 | } |
557 | } |
554 | 558 | ||
555 | matSetFull(matY, _compass, 0, Compass); |
559 | matSetFull(matY, _compass, 0, Compass); |
556 | 560 | ||
557 | /* Roll and Yaw angle are smaller 8 Deg*/ |
561 | /* Roll and Yaw angle are smaller 8 Deg*/ |
558 | if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80)) |
562 | if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80)) |
559 | { |
563 | { |
560 | fYValid[_compass] = 1; |
564 | fYValid[_compass] = 1; |
561 | } |
565 | } |
562 | else |
566 | else |
563 | { |
567 | { |
564 | fYValid[_compass] = 0; |
568 | fYValid[_compass] = 0; |
565 | } |
569 | } |
566 | #else |
570 | #else |
567 | fYValid[_compass] = 0; |
571 | fYValid[_compass] = 0; |
568 | #endif |
572 | #endif |
569 | } |
573 | } |
570 | 574 | ||
571 | /* **************************************************************************** |
575 | /* **************************************************************************** |
572 | Functionname: trUpdateBU */ /*! |
576 | Functionname: trUpdateBU */ /*! |
573 | Description: Update control matrix B and vector U |
577 | Description: Update control matrix B and vector U |
574 | 578 | ||
575 | |
579 | |
576 | @param[in] |
580 | @param[in] |
577 | 581 | ||
578 | @return void |
582 | @return void |
579 | @pre - |
583 | @pre - |
580 | @post - |
584 | @post - |
581 | @author Michael Walter |
585 | @author Michael Walter |
582 | **************************************************************************** */ |
586 | **************************************************************************** */ |
583 | void trUpdateBU() |
587 | void trUpdateBU() |
584 | { |
588 | { |
585 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
589 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
586 | 590 | ||
587 | /*--- VARIABLES ---*/ |
591 | /*--- VARIABLES ---*/ |
588 | /* |
592 | /* |
589 | dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p |
593 | dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p |
590 | dTheta = | 0 cosPhi -sinPhi | * q |
594 | dTheta = | 0 cosPhi -sinPhi | * q |
591 | dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w |
595 | dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w |
592 | */ |
596 | */ |
593 | 597 | ||
594 | static f32_t fSumGier = 0.0F; |
598 | static f32_t fSumGier = 0.0F; |
595 | static f32_t fSumNick = 0.0F; |
599 | static f32_t fSumNick = 0.0F; |
596 | static f32_t fSumRoll = 0.0F; |
600 | static f32_t fSumRoll = 0.0F; |
597 | 601 | ||
598 | f32_t ADC_p = 0.0F; |
602 | f32_t ADC_p = 0.0F; |
599 | f32_t ADC_q = 0.0F; |
603 | f32_t ADC_q = 0.0F; |
600 | f32_t ADC_r = 0.0F; |
604 | f32_t ADC_r = 0.0F; |
601 | 605 | ||
602 | /* store predicted state vector in current state */ |
606 | /* store predicted state vector in current state */ |
603 | matGetFull(matXd, _Phi , 0, &Phi); |
607 | matGetFull(matXd, _Phi , 0, &Phi); |
604 | matGetFull(matXd, _Theta , 0, &Theta); |
608 | matGetFull(matXd, _Theta , 0, &Theta); |
605 | 609 | ||
606 | sinPhi = sin(Phi); |
610 | sinPhi = sin(Phi); |
607 | cosPhi = cos(Phi); |
611 | cosPhi = cos(Phi); |
608 | 612 | ||
609 | tanTheta = tan(Theta); |
613 | tanTheta = tan(Theta); |
610 | secTheta = 1.0F / cos(Theta); |
614 | secTheta = 1.0F / cos(Theta); |
611 | 615 | ||
612 | matSetFull(matB, _Phi, _p, 1.0F); |
616 | matSetFull(matB, _Phi, _p, 1.0F); |
613 | matSetFull(matB, _Phi, _q, sinPhi * tanTheta ); |
617 | matSetFull(matB, _Phi, _q, sinPhi * tanTheta ); |
614 | matSetFull(matB, _Phi, _r, cosPhi * tanTheta ); |
618 | matSetFull(matB, _Phi, _r, cosPhi * tanTheta ); |
615 | 619 | ||
616 | //matSetFull(matB, _Theta, _p, 0.0F); |
620 | //matSetFull(matB, _Theta, _p, 0.0F); |
617 | matSetFull(matB, _Theta, _q, cosPhi ); |
621 | matSetFull(matB, _Theta, _q, cosPhi ); |
618 | matSetFull(matB, _Theta, _r, -sinPhi ); |
622 | matSetFull(matB, _Theta, _r, -sinPhi ); |
619 | 623 | ||
620 | //matSetFull(matB, _Psi, _p, 0.0F); |
624 | //matSetFull(matB, _Psi, _p, 0.0F); |
621 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
625 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
622 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
626 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
623 | 627 | ||
624 | ADC_p = -(float) AverageRoll * 0.00001F * fCycleTime; |
628 | ADC_p = -(float) AverageRoll * 0.00001F * fCycleTime; |
625 | ADC_q = -(float) AverageNick * 0.00001F * fCycleTime; |
629 | ADC_q = -(float) AverageNick * 0.00001F * fCycleTime; |
626 | ADC_r = -(float) AverageGier * 0.00001F * fCycleTime; |
630 | ADC_r = -(float) AverageGier * 0.00001F * fCycleTime; |
627 | 631 | ||
628 | fSumGier += ADC_r * 180.F / 3.14F; |
632 | fSumGier += ADC_r * 180.F / 3.14F; |
629 | fSumNick += ADC_q * 180.F / 3.14F; |
633 | fSumNick += ADC_q * 180.F / 3.14F; |
630 | fSumRoll += ADC_p * 180.F / 3.14F; |
634 | fSumRoll += ADC_p * 180.F / 3.14F; |
631 | 635 | ||
632 | DebugOut.Analog[3] = fSumNick; |
636 | DebugOut.Analog[3] = fSumNick; |
633 | DebugOut.Analog[4] = fSumRoll; |
637 | DebugOut.Analog[4] = fSumRoll; |
634 | DebugOut.Analog[5] = fSumGier; |
638 | DebugOut.Analog[5] = fSumGier; |
635 | 639 | ||
636 | matSetFull(matU, _p, 0, ADC_p ); |
640 | matSetFull(matU, _p, 0, ADC_p ); |
637 | matSetFull(matU, _q, 0, ADC_q); |
641 | matSetFull(matU, _q, 0, ADC_q); |
638 | matSetFull(matU, _r, 0, ADC_r); |
642 | matSetFull(matU, _r, 0, ADC_r); |
639 | } |
643 | } |
640 | 644 | ||
641 | 645 | ||
642 | 646 | ||
643 | /* **************************************************************************** |
647 | /* **************************************************************************** |
644 | Functionname: trLimitAngles */ /*! |
648 | Functionname: trLimitAngles */ /*! |
645 | Description: |
649 | Description: |
646 | 650 | ||
647 | @param[in] |
651 | @param[in] |
648 | 652 | ||
649 | @return void |
653 | @return void |
650 | @pre - |
654 | @pre - |
651 | @post - |
655 | @post - |
652 | @author |
656 | @author |
653 | **************************************************************************** */ |
657 | **************************************************************************** */ |
654 | void trLimitAngles() |
658 | void trLimitAngles() |
655 | { |
659 | { |
656 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
660 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
657 | 661 | ||
658 | /*--- VARIABLES ---*/ |
662 | /*--- VARIABLES ---*/ |
659 | f32_t Psi; |
663 | f32_t Psi; |
660 | 664 | ||
661 | if (status.Phi > 2.0F * PI) |
665 | if (status.Phi > 2.0F * PI) |
662 | { |
666 | { |
663 | status.Phi -= 2.0F * PI; |
667 | status.Phi -= 2.0F * PI; |
664 | matSetFull(matXs, _Phi, 0, status.Phi); |
668 | matSetFull(matXs, _Phi, 0, status.Phi); |
665 | } |
669 | } |
666 | if (status.Phi < -2.0F * PI) |
670 | if (status.Phi < -2.0F * PI) |
667 | { |
671 | { |
668 | status.Phi += 2.0F * PI; |
672 | status.Phi += 2.0F * PI; |
669 | matSetFull(matXs, _Phi, 0, status.Phi); |
673 | matSetFull(matXs, _Phi, 0, status.Phi); |
670 | } |
674 | } |
671 | if (status.Theta > 2.0F * PI) |
675 | if (status.Theta > 2.0F * PI) |
672 | { |
676 | { |
673 | status.Theta -= 2.0F * PI; |
677 | status.Theta -= 2.0F * PI; |
674 | matSetFull(matXs, _Theta, 0, status.Theta); |
678 | matSetFull(matXs, _Theta, 0, status.Theta); |
675 | } |
679 | } |
676 | if (status.Theta < -2.0F * PI) |
680 | if (status.Theta < -2.0F * PI) |
677 | { |
681 | { |
678 | status.Theta += 2.0F * PI; |
682 | status.Theta += 2.0F * PI; |
679 | matSetFull(matXs, _Theta, 0, status.Theta); |
683 | matSetFull(matXs, _Theta, 0, status.Theta); |
680 | } |
684 | } |
681 | if (status.Psi > 2.0F * PI) |
685 | if (status.Psi > 2.0F * PI) |
682 | { |
686 | { |
683 | status.Psi -= 2.0F * PI; |
687 | status.Psi -= 2.0F * PI; |
684 | sollGier -= 3600; |
688 | sollGier -= 3600; |
685 | 689 | ||
686 | matGetFull(matXs, _Psi, 0, &Psi); |
690 | matGetFull(matXs, _Psi, 0, &Psi); |
687 | matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI); |
691 | matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI); |
688 | matGetFull(matXd, _Psi, 0, &Psi); |
692 | matGetFull(matXd, _Psi, 0, &Psi); |
689 | matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI); |
693 | matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI); |
690 | } |
694 | } |
691 | if (status.Psi < 0.0F * PI) |
695 | if (status.Psi < 0.0F * PI) |
692 | { |
696 | { |
693 | status.Psi += 2.0F * PI; |
697 | status.Psi += 2.0F * PI; |
694 | sollGier += 3600; |
698 | sollGier += 3600; |
695 | 699 | ||
696 | matGetFull(matXs, _Psi, 0, &Psi); |
700 | matGetFull(matXs, _Psi, 0, &Psi); |
697 | matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI); |
701 | matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI); |
698 | matGetFull(matXd, _Psi, 0, &Psi); |
702 | matGetFull(matXd, _Psi, 0, &Psi); |
699 | matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI); |
703 | matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI); |
700 | } |
704 | } |
701 | 705 | ||
702 | status.iPhi10 = (int) (status.Phi * 573.0F); |
706 | status.iPhi10 = (int) (status.Phi * 573.0F); |
703 | status.iTheta10 = (int) (status.Theta * 573.0F); |
707 | status.iTheta10 = (int) (status.Theta * 573.0F); |
704 | status.iPsi10 = (int) (status.Psi * 573.0F); |
708 | status.iPsi10 = (int) (status.Psi * 573.0F); |
705 | 709 | ||
706 | if ((sollGier - status.iPsi10) > 3600) |
710 | if ((sollGier - status.iPsi10) > 3600) |
707 | { |
711 | { |
708 | sollGier -= 3600; |
712 | sollGier -= 3600; |
709 | } |
713 | } |
710 | 714 | ||
711 | if ((sollGier - status.iPsi10) < -3600) |
715 | if ((sollGier - status.iPsi10) < -3600) |
712 | { |
716 | { |
713 | sollGier += 3600; |
717 | sollGier += 3600; |
714 | } |
718 | } |
715 | } |
719 | } |
716 | 720 | ||
717 | 721 | ||
718 | /* **************************************************************************** |
722 | /* **************************************************************************** |
719 | Functionname: trEstimateVelocity */ /*! |
723 | Functionname: trEstimateVelocity */ /*! |
720 | Description: |
724 | Description: |
721 | 725 | ||
722 | @param[in] |
726 | @param[in] |
723 | 727 | ||
724 | @return void |
728 | @return void |
725 | @pre - |
729 | @pre - |
726 | @post - |
730 | @post - |
727 | @author |
731 | @author |
728 | **************************************************************************** */ |
732 | **************************************************************************** */ |
729 | void trEstimateVelocity() |
733 | void trEstimateVelocity() |
730 | { |
734 | { |
731 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
735 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
732 | 736 | ||
733 | /*--- VARIABLES ---*/ |
737 | /*--- VARIABLES ---*/ |
734 | - | ||
735 | } |
738 | } |
736 | 739 |