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. Time is in units of 20ms, so ticks are at 20 second intervals.

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 "kalman.h"
#include "matrix.h"
 
 
void predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y) 
{
	predict_state(model,Prev,u);
	predict_P(model,Prev);
	matrix K=optimal_gain(model,Prev);
	state_update(Prev,model,&K,y);
	P_update(&K,model,Prev);
}
 
void 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);
	state->state = vector_add(&a,&b);					//estimated state
}
 
void 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);
	state->P = 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
} 
 
void 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
	if(a.t>PI)								//need to make sure we stay in the correct range
	{
		a.t-=2*PI;
	}
	if(a.t<PI)
	{
		a.t+=2*PI;
	}
	a=matrix_vector(K,&a);
	state->state = vector_add(&state->state,&a);
}
 
void 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.0-S.tl;					//subtraction from identity
	S.tr=-S.tr;
	S.bl=-S.bl;
	S.br=1.0-S.br;
	state->P = 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;
 
void predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y);
void predict_state(kalman_model * model, kalman_state * state, vector * u);
void predict_P(kalman_model * model, kalman_state * state);
matrix optimal_gain(kalman_model * model, kalman_state * state);
void state_update(kalman_state * state, matrix * K, vector * y);
void P_update(matrix *K, kalman_model * model, kalman_state * state);

#include "matrix.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);