| 15,12 → 15,16 |
|---|
| /***************************************************************************** |
| INCLUDES |
| **************************************************************************** */ |
| #include "mat.h" |
| #include "matmatrix.h" |
| |
| /***************************************************************************** |
| (SYMBOLIC) CONSTANTS |
| *****************************************************************************/ |
| |
| /***************************************************************************** |
| VARIABLES |
| *****************************************************************************/ |
| extern int sollGier; |
| |
| /* *************************************************************************** */ |
| /* Uses the Compass to Estimate Theta, Phi, Psi */ |
| 31,19 → 35,68 |
|---|
| #define KAFI_DIM_X (3) /* number of rows of state vector */ |
| |
| #ifdef USE_Extended_MM3_Measurement_Model |
| #define KAFI_DIM_Y (6) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/ |
| #define KAFI_DIM_Y (6) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/ |
| #else |
| #define KAFI_DIM_Y (4) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/ |
| #define KAFI_DIM_Y (4) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/ |
| #endif |
| |
| #define KAFI_DIM_U (3) /* number of rows of control vector */ |
| |
| #ifdef MELEXIS_GYRO |
| #define fCycleTime 12.5F; /* Use Different Cycle Times to account for Runtime / Gain Differences */ |
| #else |
| #ifdef USE_Extended_MM3_Measurement_Model |
| #define fCycleTime 16.0F; |
| #define fCycleTime 16.0F; |
| #else |
| #define fCycleTime 10.5F; |
| #define fCycleTime 12.5F; |
| #endif |
| #endif |
| |
| /* Predict the Measurent */ |
| void trPredictMeasurement(void); |
| /* Update the Jacobi Matrix */ |
| void trUpdateJacobiMatrix(void); |
| /* Extract measurements and store them in vector matY. |
| Measurement validity is indicated by fYValid[] */ |
| void trMeasure(void); |
| /* Innovation: calculate Xd, and Pd */ |
| void trInnovate(void); |
| /* Update transition matrix Phi */ |
| void trUpdatePhi(void); |
| /* Update control matrix B */ |
| void trUpdateBU(void); |
| /* Predict new state Xs and Ps */ |
| void KAFIPrediction(void); |
| /* Main Kalman Filter Loop */ |
| void FlightAttitudeEstimation(void); |
| /* Setup the Kalman Filter Parameter */ |
| void Kafi_Init(void); |
| /* Not done yet */ |
| void trEstimateVelocity(void); |
| /* Limit Angles to [-2Pi;...;+2Pi] */ |
| void trLimitAngles(void); |
| |
| typedef struct |
| { |
| f32_t Phi; |
| f32_t Theta; |
| f32_t Psi; |
| i32_t iPhi10; |
| i32_t iTheta10; |
| i32_t iPsi10; |
| f32_t dPhi; |
| f32_t dTheta; |
| f32_t dPsi; |
| f32_t du; |
| f32_t dv; |
| f32_t dw; |
| f32_t X; |
| f32_t Y; |
| f32_t Z; |
| } status_t; |
| |
| status_t status; |
| |
| /*typedef*/ enum |
| { |
| _p = 0, |
| 75,7 → 128,6 |
|---|
| } /*trObservationVariables_e*/; |
| #endif |
| |
| |
| /*typedef*/ enum |
| { |
| _Phi = 0, |
| 88,40 → 140,7 |
|---|
| } /*trStateVariables_e*/; |
| |
| |
| typedef struct |
| { |
| f32_t Phi; |
| f32_t Theta; |
| f32_t Psi; |
| i32_t iPhi10; |
| i32_t iTheta10; |
| i32_t iPsi10; |
| f32_t dPhi; |
| f32_t dTheta; |
| f32_t dPsi; |
| f32_t du; |
| f32_t dv; |
| f32_t dw; |
| f32_t X; |
| f32_t Y; |
| f32_t Z; |
| } status_t; |
| |
| status_t status; |
| |
| |
| void trUpdatePhi(void); |
| void trUpdateBU(void); |
| void trMeasure(void); |
| void trInnovate(void); |
| |
| void trResetKalmanFilter(void); |
| void AttitudeEstimation(void); |
| |
| int KAFIInit(kafi_t *pkafi); |
| void Kafi_Init(void); |
| void trEstimateVelocity(void); |
| void trLimitAngles(void); |
| void trControl(void); |
| |
| |