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); |
|
|