UKHAS Wiki

This is an old revision of the document!

---

//optimised kalman filter code, all capitalised variables are filter parameters
//x is the two component state vector, p is the covariance
//propogator
//x=F*x+B*u - u is single component control variable
float a=F_A*x[0]+F_B*x[1]+(B_B*u);
x[1]=F_C*x[0]+F_D*x[1]+(B_D*u);
x[0]=a;
//p=F*p*F'+Q
a=F_A*(F_A*p[0][0]+F_B*p[0][1])+F_B*(F_A*p[1][0]+F_B*p[1][1]);
float b=F_A*(F_C*p[0][0]+F_D*p[0][1])+F_B*(F_C*p[1][0]+F_D*p[1][1]);
float c=F_C*(F_A*p[0][0]+F_B*p[0][1])+F_D*(F_A*p[1][0]+F_B*p[1][1]);
p[0][0]=a+Q_A;
p[0][1]=b+Q_B;
p[1][0]=c+Q_C;
//free(&c);
p[1][1]=F_C*(F_C*p[0][0]+F_D*p[0][1])+F_D*(F_C*p[1][0]+F_D*p[1][1])+Q_D;
//update
float s[2][2],y[2];
//y=z-H*x, s=p*H'
if(Heading_flag)
{
	y[0]=Heading-x[0];
	s[0][0]=p[0][0]+R_A;
	s[1][0]=p[1][0]+R_C;
	s[0][1]=p[0][1]+R_B;
}
else
{
	y[0]=0;
	s[0][0]=R_A;
	s[1][0]=R_C;
	s[0][1]=R_B;
}
y[1]=rate-x[1];
s[1][1]=p[1][1]+R_D;
//s=H'*s^-1
b=1.0/(s[0][0]*s[1][1]-s[0][1]*s[1][0]);
if(Heading_flag)
{
	a=b*s[1][1];
	s[1][1]=b*s[0][0];
	s[0][0]=a;
	s[0][1]=-b*s[0][1];
}
else
{
	s[1][1]=b*s[0][0];
	s[0][1]=0;
	s[0][0]=0;
}
s[1][0]=-b*s[1][0];
//s=p*s
a=p[0][0]*s[0][0]+p[0][1]*s[1][0];
s[1][0]=p[1][0]*s[0][0]+p[1][1]*s[1][0];
s[0][0]=a;
a=p[0][0]*s[0][1]+p[0][1]*s[1][1];
s[1][1]=p[1][0]*s[0][1]+p[1][1]*s[1][1];
s[1][0]=a;
//x=x+s*y
x[0]+=s[0][0]*y[0]+s[0][1]*y[1];
x[1]+=s[1][0]*y[0]+s[1][1]*y[1];
//free(&y);
//s=sHp
s[0][0]=s[0][1]*p[1][0];
s[0][1]=s[0][1]*p[1][1];
s[1][0]=s[1][1]*p[1][0];
s[1][1]=s[1][1]*p[1][1];
if(Heading_flag)
{
	s[0][0]+=s[0][0]*p[0][0];
	s[0][1]+=s[0][1]*p[0][1];
	s[1][0]+=s[1][0]*p[0][0];
	s[1][1]+=s[1][0]*p[0][1];
}
//p=p-s
p[0][0]-=s[0][0];
p[0][1]-=s[0][1];
p[1][0]-=s[1][0];
p[1][1]-=s[1][1];
//free(&s);
//free(&a);
//free(&b);