Rev 981 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 838 | MikeW | 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 | |||
| 15 | /***************************************************************************** |
||
| 16 | INCLUDES |
||
| 17 | **************************************************************************** */ |
||
| 966 | MikeW | 18 | #include "matmatrix.h" |
| 838 | MikeW | 19 | |
| 20 | /***************************************************************************** |
||
| 21 | (SYMBOLIC) CONSTANTS |
||
| 22 | *****************************************************************************/ |
||
| 23 | |||
| 966 | MikeW | 24 | /***************************************************************************** |
| 25 | VARIABLES |
||
| 26 | *****************************************************************************/ |
||
| 27 | extern int sollGier; |
||
| 838 | MikeW | 28 | |
| 29 | /* *************************************************************************** */ |
||
| 30 | /* Uses the Compass to Estimate Theta, Phi, Psi */ |
||
| 31 | //#define USE_Extended_MM3_Measurement_Model |
||
| 32 | /* *************************************************************************** */ |
||
| 33 | |||
| 34 | /* kalman filter model extensions change number of state variables */ |
||
| 35 | #define KAFI_DIM_X (3) /* number of rows of state vector */ |
||
| 36 | |||
| 37 | #ifdef USE_Extended_MM3_Measurement_Model |
||
| 966 | MikeW | 38 | #define KAFI_DIM_Y (6) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/ |
| 838 | MikeW | 39 | #else |
| 966 | MikeW | 40 | #define KAFI_DIM_Y (4) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/ |
| 838 | MikeW | 41 | #endif |
| 42 | |||
| 43 | #define KAFI_DIM_U (3) /* number of rows of control vector */ |
||
| 44 | |||
| 966 | MikeW | 45 | #ifdef MELEXIS_GYRO |
| 1041 | MikeW | 46 | #define fCycleTime 12.5F /* Use Different Cycle Times to account for Runtime / Gain Differences */ |
| 966 | MikeW | 47 | #else |
| 838 | MikeW | 48 | #ifdef USE_Extended_MM3_Measurement_Model |
| 1041 | MikeW | 49 | #define fCycleTime 16.0F |
| 838 | MikeW | 50 | #else |
| 1041 | MikeW | 51 | //#define fCycleTime 12.5F |
| 52 | #define fCycleTime 16.0F |
||
| 966 | MikeW | 53 | #endif |
| 838 | MikeW | 54 | #endif |
| 55 | |||
| 966 | MikeW | 56 | /* Predict the Measurent */ |
| 57 | void trPredictMeasurement(void); |
||
| 58 | /* Update the Jacobi Matrix */ |
||
| 59 | void trUpdateJacobiMatrix(void); |
||
| 60 | /* Extract measurements and store them in vector matY. |
||
| 61 | Measurement validity is indicated by fYValid[] */ |
||
| 62 | void trMeasure(void); |
||
| 63 | /* Innovation: calculate Xd, and Pd */ |
||
| 64 | void trInnovate(void); |
||
| 65 | /* Update transition matrix Phi */ |
||
| 66 | void trUpdatePhi(void); |
||
| 67 | /* Update control matrix B */ |
||
| 68 | void trUpdateBU(void); |
||
| 69 | /* Predict new state Xs and Ps */ |
||
| 70 | void KAFIPrediction(void); |
||
| 71 | /* Main Kalman Filter Loop */ |
||
| 72 | void FlightAttitudeEstimation(void); |
||
| 73 | /* Setup the Kalman Filter Parameter */ |
||
| 74 | void Kafi_Init(void); |
||
| 75 | /* Not done yet */ |
||
| 76 | void trEstimateVelocity(void); |
||
| 77 | /* Limit Angles to [-2Pi;...;+2Pi] */ |
||
| 78 | void trLimitAngles(void); |
||
| 79 | |||
| 80 | typedef struct |
||
| 81 | { |
||
| 82 | f32_t Phi; |
||
| 83 | f32_t Theta; |
||
| 84 | f32_t Psi; |
||
| 85 | i32_t iPhi10; |
||
| 86 | i32_t iTheta10; |
||
| 87 | i32_t iPsi10; |
||
| 88 | f32_t dPhi; |
||
| 89 | f32_t dTheta; |
||
| 90 | f32_t dPsi; |
||
| 91 | f32_t du; |
||
| 92 | f32_t dv; |
||
| 93 | f32_t dw; |
||
| 94 | f32_t X; |
||
| 95 | f32_t Y; |
||
| 96 | f32_t Z; |
||
| 97 | } status_t; |
||
| 98 | |||
| 99 | status_t status; |
||
| 100 | |||
| 838 | MikeW | 101 | /*typedef*/ enum |
| 102 | { |
||
| 103 | _p = 0, |
||
| 104 | _q, |
||
| 105 | _r, |
||
| 106 | c_control_variables /* number of control variables */ |
||
| 107 | } /*trControlVariables_e*/; |
||
| 108 | |||
| 109 | |||
| 110 | #ifdef USE_Extended_MM3_Measurement_Model |
||
| 111 | /*typedef*/ enum |
||
| 112 | { |
||
| 113 | _ax = 0, |
||
| 114 | _ay, |
||
| 115 | _az, |
||
| 116 | _mx, |
||
| 117 | _my, |
||
| 118 | _mz, |
||
| 119 | c_observation_variables /* number of observation variables */ |
||
| 120 | } /*trObservationVariables_e*/; |
||
| 121 | #else |
||
| 122 | /*typedef*/ enum |
||
| 123 | { |
||
| 124 | _ax = 0, |
||
| 125 | _ay, |
||
| 126 | _az, |
||
| 127 | _compass, |
||
| 128 | c_observation_variables /* number of observation variables */ |
||
| 129 | } /*trObservationVariables_e*/; |
||
| 130 | #endif |
||
| 131 | |||
| 132 | /*typedef*/ enum |
||
| 133 | { |
||
| 134 | _Phi = 0, |
||
| 135 | _Theta, |
||
| 136 | _Psi, |
||
| 137 | //_du, |
||
| 138 | //_dv, |
||
| 139 | //_dw, |
||
| 140 | c_state_variables /* number of state variables */ |
||
| 141 | } /*trStateVariables_e*/; |
||
| 142 | |||
| 143 | |||
| 144 | |||
| 145 | |||
| 146 | |||
| 147 |