UKHAS Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
 This is optimised for use with GPS/rate gyro aided heading estimators. Reduced stack, flash and clock cycle use compared to the non optimised filter. Total runtime on atmega168 is 630us
<code c>
		//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
		float s[2][2];
		s[0][0]=F_A*(F_A*p[0][0]+F_B*p[0][1])+F_B*(F_A*p[1][0]+F_B*p[1][1])+Q_A;
		s[0][1]=F_A*(F_C*p[0][0]+F_D*p[0][1])+F_B*(F_C*p[1][0]+F_D*p[1][1])+Q_B;
		s[1][0]=F_C*(F_A*p[0][0]+F_B*p[0][1])+F_D*(F_A*p[1][0]+F_B*p[1][1])+Q_C;
		s[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;
		memcpy(p,s,16);
		//update
		float y[2];
		//y=z-H*x, s=p*H'+R
		s[0][0]=R_A;
		s[1][0]=R_C;
		if(Heading_flag)
		{
			y[0]=Heading-x[0];
			s[0][0]+=p[0][0];
			s[1][0]+=p[1][0];
		}
		else
		{
			y[0]=0;
		}
		y[1]=rate-x[1];
		s[0][1]=p[0][1]+R_B;
		s[1][1]=p[1][1]+R_D;
		//s=H'*s^-1
		float 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[0][1]=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=s*H*p
		if(Heading_flag)
		{
			s[0][0]=s[0][1]*p[1][0]+s[0][0]*p[0][0];
			s[0][1]=s[0][1]*p[1][1]+s[0][1]*p[0][1];
			s[1][0]=s[1][1]*p[1][0]+s[1][0]*p[0][0];
			s[1][1]=s[1][1]*p[1][1]+s[1][0]*p[0][1];
		}
		else
		{
			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];
		}		
		//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];
</code>