Rev 966 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 966 | Rev 973 | ||
---|---|---|---|
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 | 174 | ||
175 | /* set diagonal init. estimation error (covariance) P0 */ |
175 | /* set diagonal init. estimation error (covariance) P0 */ |
176 | matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
176 | matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
177 | matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
177 | matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
178 | matSetDiagonal(matPs, _Psi, _Psi, 0.00001F); |
178 | matSetDiagonal(matPs, _Psi, _Psi, 0.00001F); |
179 | 179 | ||
180 | matSetFull(matXd, _Phi, 0, 0.0F); |
180 | matSetFull(matXd, _Phi, 0, 0.0F); |
181 | matSetFull(matXd, _Theta, 0, 0.0F); |
181 | matSetFull(matXd, _Theta, 0, 0.0F); |
182 | matSetFull(matXd, _Psi, 0, 0.0F); |
182 | matSetFull(matXd, _Psi, 0, 0.0F); |
183 | 183 | ||
184 | matSetFull(matXs, _Phi, 0, 0.0F); |
184 | matSetFull(matXs, _Phi, 0, 0.0F); |
185 | matSetFull(matXs, _Theta, 0, 0.0F); |
185 | matSetFull(matXs, _Theta, 0, 0.0F); |
186 | matSetFull(matXs, _Psi, 0, 0.0F); |
186 | matSetFull(matXs, _Psi, 0, 0.0F); |
187 | } |
187 | } |
188 | 188 | ||
189 | /* **************************************************************************** |
189 | /* **************************************************************************** |
190 | Functionname: KAFIPrediction */ /*! |
190 | Functionname: KAFIPrediction */ /*! |
191 | Description: Predict new state Xs and Ps |
191 | Description: Predict new state Xs and Ps |
192 | |
192 | |
193 | X_k = A * X_(k-1) + B * u_k |
193 | X_k = A * X_(k-1) + B * u_k |
194 | P_k = A * P_(k-1) * A^T + Q |
194 | P_k = A * P_(k-1) * A^T + Q |
195 | 195 | ||
196 | @param[in] |
196 | @param[in] |
197 | 197 | ||
198 | @return void |
198 | @return void |
199 | @pre - |
199 | @pre - |
200 | @post - |
200 | @post - |
201 | @author Michael Walter |
201 | @author Michael Walter |
202 | **************************************************************************** */ |
202 | **************************************************************************** */ |
203 | void KAFIPrediction() |
203 | void KAFIPrediction() |
204 | { |
204 | { |
205 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
205 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
206 | 206 | ||
207 | /*--- VARIABLES ---*/ |
207 | /*--- VARIABLES ---*/ |
208 | 208 | ||
209 | /* Innovation: calculate Xd, and Pd */ |
209 | /* Innovation: calculate Xd, and Pd */ |
210 | 210 | ||
211 | /* X_k = A * X_(k-1) + B * u_k */ |
211 | /* X_k = A * X_(k-1) + B * u_k */ |
212 | 212 | ||
213 | /* matX_tmp1 = matPhi * matXd */ |
213 | /* matX_tmp1 = matPhi * matXd */ |
214 | /* |
214 | /* |
215 | matrix_mult(matPhi,matXd, matX_tmp1, |
215 | matrix_mult(matPhi,matXd, matX_tmp1, |
216 | KAFI_DIM_X, KAFI_DIM_X, 1); |
216 | KAFI_DIM_X, KAFI_DIM_X, 1); |
217 | */ |
217 | */ |
218 | /* matX_tmp2 = matB * matU */ |
218 | /* matX_tmp2 = matB * matU */ |
219 | matrix_mult( matB,matU, matX_tmp2, |
219 | matrix_mult( matB,matU, matX_tmp2, |
220 | KAFI_DIM_X, KAFI_DIM_U, 1); |
220 | KAFI_DIM_X, KAFI_DIM_U, 1); |
221 | 221 | ||
222 | /* matXs = matPhi * matXd + matB * matU */ |
222 | /* matXs = matPhi * matXd + matB * matU */ |
223 | /* |
223 | /* |
224 | matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
224 | matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
225 | matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
225 | matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
226 | */ |
226 | */ |
227 | 227 | ||
228 | matXs[1][1] = matXd[1][1] + matX_tmp2[1][1]; |
228 | matXs[1][1] = matXd[1][1] + matX_tmp2[1][1]; |
229 | matXs[2][1] = matXd[2][1] + matX_tmp2[2][1]; |
229 | matXs[2][1] = matXd[2][1] + matX_tmp2[2][1]; |
230 | matXs[3][1] = matXd[3][1] + matX_tmp2[3][1]; |
230 | matXs[3][1] = matXd[3][1] + matX_tmp2[3][1]; |
231 | 231 | ||
232 | /*P_k = A * P_(k-1)*A^T + Q */ |
232 | /*P_k = A * P_(k-1)*A^T + Q */ |
233 | /* |
233 | /* |
234 | matrix_mult_transpose( matP, matPhi, matP_tmp, // Can be skiped PHI is a ident matrix |
234 | 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 ); |
235 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
236 | matrix_mult( matPhi, matP_tmp, matP, |
236 | matrix_mult( matPhi, matP_tmp, matP, |
237 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
237 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
238 | matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X ); |
238 | matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X ); |
239 | */ |
239 | */ |
240 | 240 | ||
241 | matPs[1][1] = matP[1][1] + matQ[1][1]; |
241 | matPs[1][1] = matP[1][1] + matQ[1][1]; |
242 | matPs[2][2] = matP[2][2] + matQ[2][2]; |
242 | matPs[2][2] = matP[2][2] + matQ[2][2]; |
243 | matPs[3][3] = matP[3][3] + matQ[3][3]; |
243 | matPs[3][3] = matP[3][3] + matQ[3][3]; |
244 | } |
244 | } |
245 | 245 | ||
246 | /* **************************************************************************** |
246 | /* **************************************************************************** |
247 | Functionname: trInnovate */ /*! |
247 | Functionname: trInnovate */ /*! |
248 | Description: Innovation: calculate Xd, and Pd |
248 | Description: Innovation: calculate Xd, and Pd |
249 | |
249 | |
250 | K = Ps * C^T * (C * Ps * C^T + R)^-1 |
250 | K = Ps * C^T * (C * Ps * C^T + R)^-1 |
251 | X = Xs + K (Y - Ys) |
251 | X = Xs + K (Y - Ys) |
252 | |
252 | |
253 | @param[in] |
253 | @param[in] |
254 | 254 | ||
255 | @return void |
255 | @return void |
256 | @pre - |
256 | @pre - |
257 | @post - |
257 | @post - |
258 | @author Michael Walter |
258 | @author Michael Walter |
259 | **************************************************************************** */ |
259 | **************************************************************************** */ |
260 | void trInnovate() |
260 | void trInnovate() |
261 | { |
261 | { |
262 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
262 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
263 | 263 | ||
264 | /*--- VARIABLES ---*/ |
264 | /*--- VARIABLES ---*/ |
265 | #if 0 /* Runtime optimised. To be verified */ |
265 | #if 0 /* Runtime optimised. To be verified */ |
266 | /* temp_meas_state = matC * matPs */ |
266 | /* temp_meas_state = matC * matPs */ |
267 | matrix_mult( matC, matPs, temp_meas_state, |
267 | matrix_mult( matC, matPs, temp_meas_state, |
268 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
268 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
269 | 269 | ||
270 | /* temp_meas_meas = matC * matPs^T * matC^T */ |
270 | /* temp_meas_meas = matC * matPs^T * matC^T */ |
271 | matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas, |
271 | matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas, |
272 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
272 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
273 | 273 | ||
274 | /* temp_meas_meas = matC * matPs^T * matC^T + matR */ |
274 | /* temp_meas_meas = matC * matPs^T * matC^T + matR */ |
275 | /* |
275 | /* |
276 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
276 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
277 | KAFI_DIM_Y, KAFI_DIM_Y ); |
277 | KAFI_DIM_Y, KAFI_DIM_Y ); |
278 | */ |
278 | */ |
279 | temp_meas_meas[1][1] += matR[1][1]; |
279 | temp_meas_meas[1][1] += matR[1][1]; |
280 | temp_meas_meas[2][2] += matR[2][2]; |
280 | temp_meas_meas[2][2] += matR[2][2]; |
281 | temp_meas_meas[3][3] += matR[3][3]; |
281 | temp_meas_meas[3][3] += matR[3][3]; |
282 | temp_meas_meas[4][4] += matR[4][4]; |
282 | temp_meas_meas[4][4] += matR[4][4]; |
283 | 283 | ||
284 | /* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */ |
284 | /* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */ |
285 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
285 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
286 | 286 | ||
287 | /* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
287 | /* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
288 | matrix_transpose_mult( temp_meas_state, temp_meas_2, matK, |
288 | matrix_transpose_mult( temp_meas_state, temp_meas_2, matK, |
289 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
289 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
290 | 290 | ||
291 | /* temp_state_state = matK * matC * matPs */ |
291 | /* temp_state_state = matK * matC * matPs */ |
292 | matrix_mult( matK, temp_meas_state, temp_state_state, |
292 | matrix_mult( matK, temp_meas_state, temp_state_state, |
293 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
293 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
294 | 294 | ||
295 | /* matP = matPs - matK * matC * matPs */ |
295 | /* matP = matPs - matK * matC * matPs */ |
296 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
296 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
297 | #else |
297 | #else |
298 | /* temp_state_meas = matPs * matC^T */ |
298 | /* temp_state_meas = matPs * matC^T */ |
299 | matrix_mult_transpose( matPs, matC, temp_state_meas, |
299 | matrix_mult_transpose( matPs, matC, temp_state_meas, |
300 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y ); |
300 | KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y ); |
301 | 301 | ||
302 | /* temp_meas_meas = matC * matPs * matC^T */ |
302 | /* temp_meas_meas = matC * matPs * matC^T */ |
303 | matrix_mult( matC, temp_state_meas, temp_meas_meas, |
303 | matrix_mult( matC, temp_state_meas, temp_meas_meas, |
304 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
304 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
305 | 305 | ||
306 | /* temp_meas_meas = matC * matPs * matC^T + matR */ |
306 | /* temp_meas_meas = matC * matPs * matC^T + matR */ |
307 | /* |
307 | /* |
308 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
308 | matrix_add( temp_meas_meas, matR, temp_meas_meas, |
309 | KAFI_DIM_Y, KAFI_DIM_Y ); |
309 | KAFI_DIM_Y, KAFI_DIM_Y ); |
310 | */ |
310 | */ |
311 | temp_meas_meas[1][1] += matR[1][1]; |
311 | temp_meas_meas[1][1] += matR[1][1]; |
312 | temp_meas_meas[2][2] += matR[2][2]; |
312 | temp_meas_meas[2][2] += matR[2][2]; |
313 | temp_meas_meas[3][3] += matR[3][3]; |
313 | temp_meas_meas[3][3] += matR[3][3]; |
314 | temp_meas_meas[4][4] += matR[4][4]; |
314 | temp_meas_meas[4][4] += matR[4][4]; |
315 | 315 | ||
316 | /* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */ |
316 | /* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */ |
317 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
317 | take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
318 | 318 | ||
319 | /* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
319 | /* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
320 | matrix_mult( temp_state_meas, temp_meas_2, matK, |
320 | matrix_mult( temp_state_meas, temp_meas_2, matK, |
321 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
321 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
322 | 322 | ||
323 | /* temp_meas_state = matC * matPs*/ |
323 | /* temp_meas_state = matC * matPs*/ |
324 | matrix_mult( matC, matPs, temp_meas_state, |
324 | matrix_mult( matC, matPs, temp_meas_state, |
325 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
325 | KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
326 | 326 | ||
327 | /* temp_state_state = matK * matC * matPs */ |
327 | /* temp_state_state = matK * matC * matPs */ |
328 | matrix_mult( matK, temp_meas_state, temp_state_state, |
328 | matrix_mult( matK, temp_meas_state, temp_state_state, |
329 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
329 | KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
330 | 330 | ||
331 | /* matP = matPs - matK * matC * matPs */ |
331 | /* matP = matPs - matK * matC * matPs */ |
332 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
332 | matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
333 | #endif |
333 | #endif |
334 | 334 | ||
335 | /* matdY = matY - matYs */ |
335 | /* matdY = matY - matYs */ |
336 | /* |
336 | /* |
337 | matrix_sub( matY, matYs, matdY, |
337 | matrix_sub( matY, matYs, matdY, |
338 | KAFI_DIM_Y, 1 ); |
338 | KAFI_DIM_Y, 1 ); |
339 | */ |
339 | */ |
340 | matdY[1][1] = matY[1][1] - matYs[1][1]; |
340 | matdY[1][1] = matY[1][1] - matYs[1][1]; |
341 | matdY[2][1] = matY[2][1] - matYs[2][1]; |
341 | matdY[2][1] = matY[2][1] - matYs[2][1]; |
342 | matdY[3][1] = matY[3][1] - matYs[3][1]; |
342 | matdY[3][1] = matY[3][1] - matYs[3][1]; |
343 | 343 | ||
344 | /* We will only update the Heading if we got a valid compass signal */ |
344 | /* We will only update the Heading if we got a valid compass signal */ |
345 | if(fYValid[_compass] == 1) |
345 | if(fYValid[_compass] == 1) |
346 | { |
346 | { |
347 | matdY[4][1] = matY[4][1] - matYs[4][1]; |
347 | matdY[4][1] = matY[4][1] - matYs[4][1]; |
348 | } |
348 | } |
349 | else |
349 | else |
350 | { |
350 | { |
351 | matdY[4][1] = 0.0F; |
351 | matdY[4][1] = 0.0F; |
352 | } |
352 | } |
353 | 353 | ||
354 | /* matX_tmp1 = matK * (Y -Ys) */ |
354 | /* matX_tmp1 = matK * (Y -Ys) */ |
355 | matrix_mult( matK, matdY, matX_tmp1, |
355 | matrix_mult( matK, matdY, matX_tmp1, |
356 | KAFI_DIM_X, KAFI_DIM_Y, 1 ); |
356 | KAFI_DIM_X, KAFI_DIM_Y, 1 ); |
357 | 357 | ||
358 | /* matXd = matXs + matK * (Y -Ys) */ |
358 | /* matXd = matXs + matK * (Y -Ys) */ |
359 | /* |
359 | /* |
360 | matrix_add( matXs, matX_tmp1, matXd, |
360 | matrix_add( matXs, matX_tmp1, matXd, |
361 | KAFI_DIM_X, 1 ); |
361 | KAFI_DIM_X, 1 ); |
362 | */ |
362 | */ |
363 | matXd[1][1] = matXs[1][1] + matX_tmp1[1][1]; |
363 | matXd[1][1] = matXs[1][1] + matX_tmp1[1][1]; |
364 | matXd[2][1] = matXs[2][1] + matX_tmp1[2][1]; |
364 | matXd[2][1] = matXs[2][1] + matX_tmp1[2][1]; |
365 | matXd[3][1] = matXs[3][1] + matX_tmp1[3][1]; |
365 | matXd[3][1] = matXs[3][1] + matX_tmp1[3][1]; |
366 | matXd[4][1] = matXs[4][1] + matX_tmp1[4][1]; |
366 | matXd[4][1] = matXs[4][1] + matX_tmp1[4][1]; |
367 | } |
367 | } |
368 | 368 | ||
369 | 369 | ||
370 | /* **************************************************************************** |
370 | /* **************************************************************************** |
371 | Functionname: trUpdatePhi */ /*! |
371 | Functionname: trUpdatePhi */ /*! |
372 | Description: Setup matPhi, used to calculate the predicted state |
372 | Description: Setup matPhi, used to calculate the predicted state |
373 | from the current state |
373 | from the current state |
374 | |
374 | |
375 | @param[in] |
375 | @param[in] |
376 | @param[in] fCycleTime |
376 | @param[in] fCycleTime |
377 | 377 | ||
378 | @return void |
378 | @return void |
379 | @pre - |
379 | @pre - |
380 | @post - |
380 | @post - |
381 | @author Michael Walter |
381 | @author Michael Walter |
382 | **************************************************************************** */ |
382 | **************************************************************************** */ |
383 | void trUpdatePhi() |
383 | void trUpdatePhi() |
384 | { |
384 | { |
385 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
385 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
386 | 386 | ||
387 | /*--- VARIABLES ---*/ |
387 | /*--- VARIABLES ---*/ |
388 | matSetFull(matPhi, _Phi, _Phi, 1.0F); |
388 | matSetFull(matPhi, _Phi, _Phi, 1.0F); |
389 | matSetFull(matPhi, _Theta, _Theta, 1.0F); |
389 | matSetFull(matPhi, _Theta, _Theta, 1.0F); |
390 | matSetFull(matPhi, _Psi, _Psi, 1.0F); |
390 | matSetFull(matPhi, _Psi, _Psi, 1.0F); |
391 | } |
391 | } |
392 | 392 | ||
393 | /* **************************************************************************** |
393 | /* **************************************************************************** |
394 | Functionname: trUpdateJacobiMatrix */ /*! |
394 | Functionname: trUpdateJacobiMatrix */ /*! |
395 | Description: |
395 | Description: |
396 | |
396 | |
397 | @param[in] void |
397 | @param[in] void |
398 | 398 | ||
399 | @return void |
399 | @return void |
400 | @pre - |
400 | @pre - |
401 | @post - |
401 | @post - |
402 | @author Michael Walter |
402 | @author Michael Walter |
403 | *****************************************************************************/ |
403 | *****************************************************************************/ |
404 | void trUpdateJacobiMatrix(void) |
404 | void trUpdateJacobiMatrix(void) |
405 | { |
405 | { |
406 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
406 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
407 | /* |
407 | /* |
408 | Simplified Version d/dt |
408 | Simplified Version d/dt |
409 | Pre_ax = du - sinTheta * g; |
409 | Pre_ax = du - sinTheta * g; |
410 | Pre_ay = dv + cosTheta_sinPhi * g; |
410 | Pre_ay = dv + cosTheta_sinPhi * g; |
411 | Pre_az = dw + cosTheta_cosPhi * g; |
411 | Pre_az = dw + cosTheta_cosPhi * g; |
412 | */ |
412 | */ |
413 | 413 | ||
414 | /*--- VARIABLES ---*/ |
414 | /*--- VARIABLES ---*/ |
415 | f32_t Phi, Theta, Psi; |
415 | f32_t Phi, Theta, Psi; |
416 | f32_t g = 9.81F; |
416 | f32_t g = 9.81F; |
417 | 417 | ||
418 | Phi = status.Phi; |
418 | Phi = status.Phi; |
419 | Theta = status.Theta; |
419 | Theta = status.Theta; |
420 | Psi = status.Psi; |
420 | Psi = status.Psi; |
421 | 421 | ||
422 | sinPhi = sin(Phi); |
422 | sinPhi = sin(Phi); |
423 | cosPhi = cos(Phi); |
423 | cosPhi = cos(Phi); |
424 | sinTheta = sin(Theta); |
424 | sinTheta = sin(Theta); |
425 | cosTheta = cos(Theta); |
425 | cosTheta = cos(Theta); |
426 | 426 | ||
427 | //Pre_ax = du - sinTheta * g; |
427 | //Pre_ax = du - sinTheta * g; |
428 | //matSetFull(matC, _ax, _Phi , 0); |
428 | //matSetFull(matC, _ax, _Phi , 0); |
429 | matSetFull(matC, _ax, _Theta, -cosTheta * g); |
429 | matSetFull(matC, _ax, _Theta, -cosTheta * g); |
430 | //matSetFull(matC, _ax, _Psi , 0); |
430 | //matSetFull(matC, _ax, _Psi , 0); |
431 | 431 | ||
432 | //Pre_ay = dv + cosTheta_sinPhi * g; |
432 | //Pre_ay = dv + cosTheta_sinPhi * g; |
433 | matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
433 | matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
434 | matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
434 | matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
435 | //matSetFull(matC, _ay, _Psi , 0); |
435 | //matSetFull(matC, _ay, _Psi , 0); |
436 | 436 | ||
437 | //Pre_az = dw + cosTheta_cosPhi * g; |
437 | //Pre_az = dw + cosTheta_cosPhi * g; |
438 | matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
438 | matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
439 | matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
439 | matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
440 | //matSetFull(matC, _az, _Psi , 0); |
440 | //matSetFull(matC, _az, _Psi , 0); |
441 | 441 | ||
442 | matSetFull(matC, _compass, _Psi , 1.0F); |
442 | matSetFull(matC, _compass, _Psi , 1.0F); |
443 | } |
443 | } |
444 | 444 | ||
445 | /***************************************************************************** |
445 | /***************************************************************************** |
446 | Functionname: trPredictMeasurement */ /*! |
446 | Functionname: trPredictMeasurement */ /*! |
447 | Description:Predict Measurements: AX, AY, AZ, Compass |
447 | Description:Predict Measurements: AX, AY, AZ, Compass |
448 | 448 | ||
449 | |
449 | |
450 | @param[in] void |
450 | @param[in] void |
451 | 451 | ||
452 | @return void |
452 | @return void |
453 | @pre - |
453 | @pre - |
454 | @post - |
454 | @post - |
455 | @author Michael Walter |
455 | @author Michael Walter |
456 | *****************************************************************************/ |
456 | *****************************************************************************/ |
457 | void trPredictMeasurement(void) |
457 | void trPredictMeasurement(void) |
458 | { |
458 | { |
459 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
459 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
460 | 460 | ||
461 | /* |
461 | /* |
462 | Simplified Version |
462 | Simplified Version |
463 | Pre_ax = du - sinTheta * g; |
463 | Pre_ax = du - sinTheta * g; |
464 | Pre_ay = dv + cosTheta_sinPhi * g; |
464 | Pre_ay = dv + cosTheta_sinPhi * g; |
465 | Pre_az = dw + cosTheta_cosPhi * g; |
465 | Pre_az = dw + cosTheta_cosPhi * g; |
466 | */ |
466 | */ |
467 | 467 | ||
468 | /*--- VARIABLES ---*/ |
468 | /*--- VARIABLES ---*/ |
469 | f32_t g = 9.81F; |
469 | f32_t g = 9.81F; |
470 | f32_t Pre_ax, Pre_ay, Pre_az; |
470 | f32_t Pre_ax, Pre_ay, Pre_az; |
471 | f32_t Phi, Theta, Psi; |
471 | f32_t Phi, Theta, Psi; |
472 | 472 | ||
473 | matGetFull(matXs, _Phi, 0, &Phi); |
473 | matGetFull(matXs, _Phi, 0, &Phi); |
474 | matGetFull(matXs, _Theta, 0, &Theta); |
474 | matGetFull(matXs, _Theta, 0, &Theta); |
475 | matGetFull(matXs, _Psi, 0, &Psi); |
475 | matGetFull(matXs, _Psi, 0, &Psi); |
476 | 476 | ||
477 | sinPhi_P = sin(Phi); |
477 | sinPhi_P = sin(Phi); |
478 | cosPhi_P = cos(Phi); |
478 | cosPhi_P = cos(Phi); |
479 | sinTheta_P = sin(Theta); |
479 | sinTheta_P = sin(Theta); |
480 | cosTheta_P = cos(Theta); |
480 | cosTheta_P = cos(Theta); |
481 | 481 | ||
482 | /* Simplified Version */ |
482 | /* Simplified Version */ |
483 | Pre_ax = -sinTheta_P * g; |
483 | Pre_ax = -sinTheta_P * g; |
484 | Pre_ay = cosTheta_P * sinPhi_P * g; |
484 | Pre_ay = cosTheta_P * sinPhi_P * g; |
485 | Pre_az = cosTheta_P * cosPhi_P * g; |
485 | Pre_az = cosTheta_P * cosPhi_P * g; |
486 | 486 | ||
487 | matSetFull(matYs, _ax, 0, Pre_ax); |
487 | matSetFull(matYs, _ax, 0, Pre_ax); |
488 | matSetFull(matYs, _ay, 0, Pre_ay); |
488 | matSetFull(matYs, _ay, 0, Pre_ay); |
489 | matSetFull(matYs, _az, 0, Pre_az); |
489 | matSetFull(matYs, _az, 0, Pre_az); |
490 | matSetFull(matYs, _compass, 0, Psi); |
490 | matSetFull(matYs, _compass, 0, Psi); |
491 | } |
491 | } |
492 | 492 | ||
493 | /* **************************************************************************** |
493 | /* **************************************************************************** |
494 | Functionname: trMeasure */ /*! |
494 | Functionname: trMeasure */ /*! |
495 | Description: |
495 | Description: |
496 | 496 | ||
497 | @param[in] |
497 | @param[in] |
498 | |
498 | |
499 | @return void |
499 | @return void |
500 | @pre - |
500 | @pre - |
501 | @post - |
501 | @post - |
502 | @author Michael Walter |
502 | @author Michael Walter |
503 | **************************************************************************** */ |
503 | **************************************************************************** */ |
504 | void trMeasure() |
504 | void trMeasure() |
505 | { |
505 | { |
506 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
506 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
507 | 507 | ||
508 | /*--- VARIABLES ---*/ |
508 | /*--- VARIABLES ---*/ |
509 | #ifdef USE_COMPASS |
509 | #ifdef USE_COMPASS |
510 | static int updCompass = 10; |
510 | static int updCompass = 10; |
511 | f32_t Psi, Compass; |
511 | f32_t Psi, Compass; |
512 | #endif |
512 | #endif |
513 | 513 | ||
514 | f32_t ADC_ax = 0.0F; |
514 | f32_t ADC_ax = 0.0F; |
515 | f32_t ADC_ay = 0.0F; |
515 | f32_t ADC_ay = 0.0F; |
516 | f32_t ADC_az = 0.0F; |
516 | f32_t ADC_az = 0.0F; |
517 | 517 | ||
518 | fYValid[_ax] = 1; |
518 | fYValid[_ax] = 1; |
519 | fYValid[_ay] = 1; |
519 | fYValid[_ay] = 1; |
520 | fYValid[_az] = 1; |
520 | fYValid[_az] = 1; |
521 | 521 | ||
522 | #ifdef INTERNAL_REFERENCE |
522 | #ifdef INTERNAL_REFERENCE |
523 | ADC_ax = AdWertAccNick * 0.044F; |
523 | ADC_ax = AdWertAccNick * 0.044F; |
524 | ADC_ay = -AdWertAccRoll * 0.044F; |
524 | ADC_ay = -AdWertAccRoll * 0.044F; |
525 | ADC_az = AdWertAccHoch * 0.044F; |
525 | ADC_az = AdWertAccHoch * 0.044F; |
526 | #else |
526 | #else |
527 | ADC_ax = AdWertAccNick * 0.047F; |
527 | ADC_ax = AdWertAccNick * 0.047F; |
528 | ADC_ay = -AdWertAccRoll * 0.047F; |
528 | ADC_ay = -AdWertAccRoll * 0.047F; |
529 | ADC_az = AdWertAccHoch * 0.047F; |
529 | ADC_az = AdWertAccHoch * 0.047F; |
530 | #endif |
530 | #endif |
531 | 531 | ||
532 | matSetFull(matY, _ax, 0, ADC_ax); |
532 | matSetFull(matY, _ax, 0, ADC_ax); |
533 | matSetFull(matY, _ay, 0, ADC_ay); |
533 | matSetFull(matY, _ay, 0, ADC_ay); |
534 | matSetFull(matY, _az, 0, ADC_az); |
534 | matSetFull(matY, _az, 0, ADC_az); |
535 | 535 | ||
536 | #ifdef USE_COMPASS |
536 | #ifdef USE_COMPASS |
537 | if (!updCompass--) |
537 | if (!updCompass--) |
538 | { |
538 | { |
539 | updCompass = 10; // update only at 2ms*50 = 100ms (10Hz) |
539 | updCompass = 10; // update only at 2ms*50 = 100ms (10Hz) |
540 | KompassValue = MM3_Heading(); |
540 | KompassValue = MM3_Heading(); |
541 | } |
541 | } |
542 | Compass = KompassValue / 180.F * PI; |
542 | Compass = KompassValue / 180.F * PI; |
543 | 543 | ||
544 | matGetFull(matXs, _Psi, 0, &Psi); |
544 | matGetFull(matXs, _Psi, 0, &Psi); |
545 | 545 | ||
546 | if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi)) |
546 | if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi)) |
547 | { |
547 | { |
548 | Compass += 2 * PI; |
548 | Compass += 2 * PI; |
549 | } |
549 | } |
550 | else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi)) |
550 | else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi)) |
551 | { |
551 | { |
552 | Compass -= 2 * PI; |
552 | Compass -= 2 * PI; |
553 | } |
553 | } |
554 | 554 | ||
555 | matSetFull(matY, _compass, 0, Compass); |
555 | matSetFull(matY, _compass, 0, Compass); |
556 | 556 | ||
557 | /* Roll and Yaw angle are smaller 8 Deg*/ |
557 | /* Roll and Yaw angle are smaller 8 Deg*/ |
558 | if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80)) |
558 | if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80)) |
559 | { |
559 | { |
560 | fYValid[_compass] = 1; |
560 | fYValid[_compass] = 1; |
561 | } |
561 | } |
562 | else |
562 | else |
563 | { |
563 | { |
564 | fYValid[_compass] = 0; |
564 | fYValid[_compass] = 0; |
565 | } |
565 | } |
566 | #else |
566 | #else |
567 | fYValid[_compass] = 0; |
567 | fYValid[_compass] = 0; |
568 | #endif |
568 | #endif |
569 | } |
569 | } |
570 | 570 | ||
571 | /* **************************************************************************** |
571 | /* **************************************************************************** |
572 | Functionname: trUpdateBU */ /*! |
572 | Functionname: trUpdateBU */ /*! |
573 | Description: Update control matrix B and vector U |
573 | Description: Update control matrix B and vector U |
574 | 574 | ||
575 | |
575 | |
576 | @param[in] |
576 | @param[in] |
577 | 577 | ||
578 | @return void |
578 | @return void |
579 | @pre - |
579 | @pre - |
580 | @post - |
580 | @post - |
581 | @author Michael Walter |
581 | @author Michael Walter |
582 | **************************************************************************** */ |
582 | **************************************************************************** */ |
583 | void trUpdateBU() |
583 | void trUpdateBU() |
584 | { |
584 | { |
585 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
585 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
586 | 586 | ||
587 | /*--- VARIABLES ---*/ |
587 | /*--- VARIABLES ---*/ |
588 | /* |
588 | /* |
589 | dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p |
589 | dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p |
590 | dTheta = | 0 cosPhi -sinPhi | * q |
590 | dTheta = | 0 cosPhi -sinPhi | * q |
591 | dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w |
591 | dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w |
592 | */ |
592 | */ |
593 | 593 | ||
594 | static f32_t fSumGier = 0.0F; |
594 | static f32_t fSumGier = 0.0F; |
595 | static f32_t fSumNick = 0.0F; |
595 | static f32_t fSumNick = 0.0F; |
596 | static f32_t fSumRoll = 0.0F; |
596 | static f32_t fSumRoll = 0.0F; |
597 | 597 | ||
598 | f32_t ADC_p = 0.0F; |
598 | f32_t ADC_p = 0.0F; |
599 | f32_t ADC_q = 0.0F; |
599 | f32_t ADC_q = 0.0F; |
600 | f32_t ADC_r = 0.0F; |
600 | f32_t ADC_r = 0.0F; |
601 | 601 | ||
602 | /* store predicted state vector in current state */ |
602 | /* store predicted state vector in current state */ |
603 | matGetFull(matXd, _Phi , 0, &Phi); |
603 | matGetFull(matXd, _Phi , 0, &Phi); |
604 | matGetFull(matXd, _Theta , 0, &Theta); |
604 | matGetFull(matXd, _Theta , 0, &Theta); |
605 | 605 | ||
606 | sinPhi = sin(Phi); |
606 | sinPhi = sin(Phi); |
607 | cosPhi = cos(Phi); |
607 | cosPhi = cos(Phi); |
608 | 608 | ||
609 | tanTheta = tan(Theta); |
609 | tanTheta = tan(Theta); |
610 | secTheta = 1.0F / cos(Theta); |
610 | secTheta = 1.0F / cos(Theta); |
611 | 611 | ||
612 | matSetFull(matB, _Phi, _p, 1.0F); |
612 | matSetFull(matB, _Phi, _p, 1.0F); |
613 | matSetFull(matB, _Phi, _q, sinPhi * tanTheta ); |
613 | matSetFull(matB, _Phi, _q, sinPhi * tanTheta ); |
614 | matSetFull(matB, _Phi, _r, cosPhi * tanTheta ); |
614 | matSetFull(matB, _Phi, _r, cosPhi * tanTheta ); |
615 | 615 | ||
616 | //matSetFull(matB, _Theta, _p, 0.0F); |
616 | //matSetFull(matB, _Theta, _p, 0.0F); |
617 | matSetFull(matB, _Theta, _q, cosPhi ); |
617 | matSetFull(matB, _Theta, _q, cosPhi ); |
618 | matSetFull(matB, _Theta, _r, -sinPhi ); |
618 | matSetFull(matB, _Theta, _r, -sinPhi ); |
619 | 619 | ||
620 | //matSetFull(matB, _Psi, _p, 0.0F); |
620 | //matSetFull(matB, _Psi, _p, 0.0F); |
621 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
621 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
622 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
622 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
623 | 623 | ||
624 | ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime; |
624 | ADC_p = -(float) AverageRoll * 0.00001F * fCycleTime; |
625 | ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime; |
625 | ADC_q = -(float) AverageNick * 0.00001F * fCycleTime; |
626 | ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime; |
626 | ADC_r = -(float) AverageGier * 0.00001F * fCycleTime; |
627 | 627 | ||
628 | fSumGier += ADC_r * 180.F / 3.14F; |
628 | fSumGier += ADC_r * 180.F / 3.14F; |
629 | fSumNick += ADC_q * 180.F / 3.14F; |
629 | fSumNick += ADC_q * 180.F / 3.14F; |
630 | fSumRoll += ADC_p * 180.F / 3.14F; |
630 | fSumRoll += ADC_p * 180.F / 3.14F; |
631 | 631 | ||
632 | DebugOut.Analog[3] = fSumNick; |
632 | DebugOut.Analog[3] = fSumNick; |
633 | DebugOut.Analog[4] = fSumRoll; |
633 | DebugOut.Analog[4] = fSumRoll; |
634 | DebugOut.Analog[5] = fSumGier; |
634 | DebugOut.Analog[5] = fSumGier; |
635 | 635 | ||
636 | matSetFull(matU, _p, 0, ADC_p ); |
636 | matSetFull(matU, _p, 0, ADC_p ); |
637 | matSetFull(matU, _q, 0, ADC_q); |
637 | matSetFull(matU, _q, 0, ADC_q); |
638 | matSetFull(matU, _r, 0, ADC_r); |
638 | matSetFull(matU, _r, 0, ADC_r); |
639 | } |
639 | } |
640 | 640 | ||
641 | 641 | ||
642 | 642 | ||
643 | /* **************************************************************************** |
643 | /* **************************************************************************** |
644 | Functionname: trLimitAngles */ /*! |
644 | Functionname: trLimitAngles */ /*! |
645 | Description: |
645 | Description: |
646 | 646 | ||
647 | @param[in] |
647 | @param[in] |
648 | 648 | ||
649 | @return void |
649 | @return void |
650 | @pre - |
650 | @pre - |
651 | @post - |
651 | @post - |
652 | @author |
652 | @author |
653 | **************************************************************************** */ |
653 | **************************************************************************** */ |
654 | void trLimitAngles() |
654 | void trLimitAngles() |
655 | { |
655 | { |
656 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
656 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
657 | 657 | ||
658 | /*--- VARIABLES ---*/ |
658 | /*--- VARIABLES ---*/ |
659 | f32_t Psi; |
659 | f32_t Psi; |
660 | 660 | ||
661 | if (status.Phi > 2.0F * PI) |
661 | if (status.Phi > 2.0F * PI) |
662 | { |
662 | { |
663 | status.Phi -= 2.0F * PI; |
663 | status.Phi -= 2.0F * PI; |
664 | matSetFull(matXs, _Phi, 0, status.Phi); |
664 | matSetFull(matXs, _Phi, 0, status.Phi); |
665 | } |
665 | } |
666 | if (status.Phi < -2.0F * PI) |
666 | if (status.Phi < -2.0F * PI) |
667 | { |
667 | { |
668 | status.Phi += 2.0F * PI; |
668 | status.Phi += 2.0F * PI; |
669 | matSetFull(matXs, _Phi, 0, status.Phi); |
669 | matSetFull(matXs, _Phi, 0, status.Phi); |
670 | } |
670 | } |
671 | if (status.Theta > 2.0F * PI) |
671 | if (status.Theta > 2.0F * PI) |
672 | { |
672 | { |
673 | status.Theta -= 2.0F * PI; |
673 | status.Theta -= 2.0F * PI; |
674 | matSetFull(matXs, _Theta, 0, status.Theta); |
674 | matSetFull(matXs, _Theta, 0, status.Theta); |
675 | } |
675 | } |
676 | if (status.Theta < -2.0F * PI) |
676 | if (status.Theta < -2.0F * PI) |
677 | { |
677 | { |
678 | status.Theta += 2.0F * PI; |
678 | status.Theta += 2.0F * PI; |
679 | matSetFull(matXs, _Theta, 0, status.Theta); |
679 | matSetFull(matXs, _Theta, 0, status.Theta); |
680 | } |
680 | } |
681 | if (status.Psi > 2.0F * PI) |
681 | if (status.Psi > 2.0F * PI) |
682 | { |
682 | { |
683 | status.Psi -= 2.0F * PI; |
683 | status.Psi -= 2.0F * PI; |
684 | sollGier -= 3600; |
684 | sollGier -= 3600; |
685 | 685 | ||
686 | matGetFull(matXs, _Psi, 0, &Psi); |
686 | matGetFull(matXs, _Psi, 0, &Psi); |
687 | matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI); |
687 | matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI); |
688 | matGetFull(matXd, _Psi, 0, &Psi); |
688 | matGetFull(matXd, _Psi, 0, &Psi); |
689 | matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI); |
689 | matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI); |
690 | } |
690 | } |
691 | if (status.Psi < 0.0F * PI) |
691 | if (status.Psi < 0.0F * PI) |
692 | { |
692 | { |
693 | status.Psi += 2.0F * PI; |
693 | status.Psi += 2.0F * PI; |
694 | sollGier += 3600; |
694 | sollGier += 3600; |
695 | 695 | ||
696 | matGetFull(matXs, _Psi, 0, &Psi); |
696 | matGetFull(matXs, _Psi, 0, &Psi); |
697 | matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI); |
697 | matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI); |
698 | matGetFull(matXd, _Psi, 0, &Psi); |
698 | matGetFull(matXd, _Psi, 0, &Psi); |
699 | matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI); |
699 | matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI); |
700 | } |
700 | } |
701 | 701 | ||
702 | status.iPhi10 = (int) (status.Phi * 573.0F); |
702 | status.iPhi10 = (int) (status.Phi * 573.0F); |
703 | status.iTheta10 = (int) (status.Theta * 573.0F); |
703 | status.iTheta10 = (int) (status.Theta * 573.0F); |
704 | status.iPsi10 = (int) (status.Psi * 573.0F); |
704 | status.iPsi10 = (int) (status.Psi * 573.0F); |
705 | 705 | ||
706 | if ((sollGier - status.iPsi10) > 3600) |
706 | if ((sollGier - status.iPsi10) > 3600) |
707 | { |
707 | { |
708 | sollGier -= 3600; |
708 | sollGier -= 3600; |
709 | } |
709 | } |
710 | 710 | ||
711 | if ((sollGier - status.iPsi10) < -3600) |
711 | if ((sollGier - status.iPsi10) < -3600) |
712 | { |
712 | { |
713 | sollGier += 3600; |
713 | sollGier += 3600; |
714 | } |
714 | } |
715 | } |
715 | } |
716 | 716 | ||
717 | 717 | ||
718 | /* **************************************************************************** |
718 | /* **************************************************************************** |
719 | Functionname: trEstimateVelocity */ /*! |
719 | Functionname: trEstimateVelocity */ /*! |
720 | Description: |
720 | Description: |
721 | 721 | ||
722 | @param[in] |
722 | @param[in] |
723 | 723 | ||
724 | @return void |
724 | @return void |
725 | @pre - |
725 | @pre - |
726 | @post - |
726 | @post - |
727 | @author |
727 | @author |
728 | **************************************************************************** */ |
728 | **************************************************************************** */ |
729 | void trEstimateVelocity() |
729 | void trEstimateVelocity() |
730 | { |
730 | { |
731 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
731 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
732 | 732 | ||
733 | /*--- VARIABLES ---*/ |
733 | /*--- VARIABLES ---*/ |
734 | 734 | ||
735 | } |
735 | } |
736 | 736 |