UKHAS Wiki

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