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