UKHAS Wiki

Test with data for an oscillating parafoil (hopefully won't happen). Green is the filter heading output, pink GPS heading, red gyro output, and blue filtered rate output.

The simulated gps heading is shown in blue, with the kalman filter output for heading in red, and turn rate in green.

The simulated gyro output in red, and filter turn rate output in green.

#include "main.h"
 
 
kalman_state predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y) 
{
	kalman_state New;
	matrix K;
	New.state=predict_state(model,Prev,u);
	New.P=predict_P(model,Prev);
	K=optimal_gain(model,Prev);
	New.state=state_update(Prev,model,&K,y);
	New.P=P_update(&K,model,Prev);
	return New;
}
 
vector predict_state(kalman_model * model, kalman_state * state, vector * u) //x is system state, u control input(s), B input model
{
	vector a;
	vector b;
	a=matrix_vector(&model->F,&state->state);
	b=matrix_vector(&model->B,u);
	return vector_add(&a,&b);								//estimated state
}
 
matrix predict_P(kalman_model * model, kalman_state * state)		//F is propagation model, Q model noise
{
	matrix S=transpose(&model->F);
	S=multiply(&state->P,&S);
	S=multiply(&model->F,&S);
	return add(&S,&model->Q);						//estimated estimate covariance
}
 
matrix optimal_gain(kalman_model * model, kalman_state * state)		//H is measurement model, R measurement noise
{
	matrix T=transpose(&model->H);
	matrix S=multiply(&state->P,&T);
	S=multiply(&model->H,&S);
	S=add(&model->R,&S);					//S is now the innovation covariance	
	S=inverse(&S);
	S=multiply(&T,&S);
	return multiply(&state->P,&S);			//the optimal gain
} 
 
vector state_update(kalman_state * state, kalman_model * model, matrix * K, vector * y)	//y is the measurements
{
	vector a=matrix_vector(&model->H,&state->state);
	a=vector_subtract(y,&a);					//a is now the difference or residual
	matrix_vector(K,&a);
	return vector_add(&state->state,&a);
}
 
matrix P_update(matrix *K, kalman_model * model, kalman_state * state)		//K is optimal gain, H measurement model
{
	matrix S=multiply(K,&model->H);
	S.tl=1-S.tl;					//subtraction from identity
	S.tr=-S.tr;
	S.bl=-S.bl;
	S.br=1-S.br;
	return multiply(&S,&state->P);
}

typedef struct
{
	vector state;	//system state
	matrix P;		//estimate covariance
} kalman_state;
 
typedef struct
{
	matrix H;	//measurement model
	matrix B;	//control model
	matrix F;	//propogation model
	matrix Q;	//process noise
	matrix R;	//measurement moise
} kalman_model;
 
kalman_state predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y);
vector predict_state(kalman_model * model, kalman_state * state, vector * u);
matrix predict_P(kalman_model * model, kalman_state * state);
matrix optimal_gain(kalman_model * model, kalman_state * state);
vector state_update(kalman_state * state, matrix * K, vector * y);
matrix P_update(matrix *K, kalman_model * model, kalman_state * state);

#include "main.h"
//matrix operations
 
matrix multiply(matrix * a, matrix * b)
{
	matrix ans;
	ans.tl=(a->tl*b->tl)+(a->tr*b->bl);
	ans.tr=(a->tl*b->tr)+(a->tr*b->br);
	ans.bl=(a->bl*b->tl)+(a->br*b->bl);
	ans.br=(a->bl*b->tr)+(a->br*b->br);
	return ans;
}
 
matrix inverse(matrix * a)
{
	matrix ans;
	float factor;
	factor=1.0/((a->tl*a->br)-(a->bl*a->tr));
	ans.tl=factor*a->br;
	ans.tr=-factor*a->tr;
	ans.bl=-factor*a->bl;
	ans.br=factor*a->tl;
	return ans;
}
 
matrix add(matrix * a, matrix * b)
{
	matrix ans;
	ans.tl=a->tl+b->tl;
	ans.tr=a->tr+b->tr;
	ans.bl=a->bl+b->bl;
	ans.br=a->br+b->br;
	return ans;
}
 
matrix subtract(matrix * a, matrix * b)
{
	matrix ans;
	ans.tl=a->tl-b->tl;
	ans.tr=a->tr-b->tr;
	ans.bl=a->bl-b->bl;
	ans.br=a->br-b->br;
	return ans;
}
 
matrix transpose(matrix * a)
{
	matrix ans;
	ans.tl=a->tl;
	ans.tr=a->bl;
	ans.bl=a->tr;
	ans.br=a->br;
	return ans;
}
 
vector matrix_vector(matrix * a, vector * b)
{
	vector ans;
	ans.t=(a->tl*b->t)+(a->tr*b->b);
	ans.b=(a->bl*b->t)+(a->br*b->b);
	return ans;
}
 
vector vector_add(vector * a, vector * b)
{
	vector ans;
	ans.t=a->t+b->t;
	ans.b=a->b+b->b;
	return ans;
}
 
vector vector_subtract(vector * a, vector * b)
{
	vector ans;
	ans.t=a->t-b->t;
	ans.b=a->b-b->b;
	return ans;
}

typedef struct
{
	float tl;
	float tr;
	float bl;
	float br;
} matrix;
 
typedef struct
{
	float t;
	float b;
} vector;
 
 
matrix multiply(matrix * a, matrix * b);
matrix inverse(matrix * a);
matrix add(matrix * a, matrix * b);
matrix subtract(matrix * a, matrix * b);
matrix transpose(matrix * a);
vector matrix_vector(matrix * a, vector * b);
vector vector_add(vector * a, vector * b);