WebSVN - FlightCtrl - Diff - Rev 981 and 1041 - /branches/KalmanFilter MikeW/kafi.h


/*
Copyright 2008, by Michael Walter
 
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
License along with this program. If not, see <http://www.gnu.org/licenses/>.
 
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/
 
/*****************************************************************************
  INCLUDES
**************************************************************************** */
#include "matmatrix.h"
 
/*****************************************************************************
  (SYMBOLIC) CONSTANTS
*****************************************************************************/
 
/*****************************************************************************
  VARIABLES
*****************************************************************************/
extern int sollGier;
 
/* *************************************************************************** */
/* Uses the Compass to Estimate Theta, Phi, Psi  */
//#define USE_Extended_MM3_Measurement_Model
/* *************************************************************************** */
 
/* kalman filter model extensions change number of state variables */
#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)*/
#else
    #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
#else
    //#define fCycleTime 12.5F
    #define fCycleTime 16.0F
    #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,
   _q,
   _r,
   c_control_variables                       /* number of control variables */
} /*trControlVariables_e*/;
 
 
#ifdef USE_Extended_MM3_Measurement_Model
/*typedef*/ enum
{
  _ax = 0,
  _ay,
  _az,
  _mx,
  _my,
  _mz,
  c_observation_variables                       /* number of observation variables */
} /*trObservationVariables_e*/;
#else
/*typedef*/ enum
{
  _ax = 0,
  _ay,
  _az,
  _compass,
  c_observation_variables                       /* number of observation variables */
} /*trObservationVariables_e*/;
#endif
 
/*typedef*/ enum
{
   _Phi = 0,
   _Theta,
   _Psi,
   //_du,
   //_dv,
   //_dw,
   c_state_variables                       /* number of state variables */
} /*trStateVariables_e*/;
 
 
 
 
 
 
 

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

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/kafi.h - Rev 981 → 1041