UKHAS Wiki

This is an old revision of the document!

---

		//kalman_state our_state_;
 
		//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=ps
 
		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);