UKHAS Wiki

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

		//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];