Differences

This shows you the differences between two versions of the page.

--- code:optimised_kalman [2008/11/30 00:55] – laurenceb
+++ code:optimised_kalman [2008/11/30 05:21] (current) – laurenceb
@@ Line 1: / Line 1: @@
+ 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
+		//Optimised Kalman filter code, all capitalised variables are filter parameters
-//x is the two component state vector, p is the covariance
+		//x is the two component state vector, p is the covariance
-//propogator
+		//propogator
-//x=F*x+B*u - u is single component control variable
+		//x=F*x+B*u - u is single component control variable
-float a=F_A*x[0]+F_B*x[1]+(B_B*u);
+		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[1]=F_C*x[0]+F_D*x[1]+(B_D*u);
-x[0]=a;
+		x[0]=a;
-//p=F*p*F'+Q
+		//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 s[2][2];
-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]);
+		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;
-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]);
+		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;
-p[0][0]=a+Q_A;
+		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;
-p[0][1]=b+Q_B;
+		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;
-p[1][0]=c+Q_C;
+		memcpy(p,s,16);
-//free(&c);
+		//update
-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;
+		float y[2];
-//update
+		//y=z-H*x, s=p*H'+R
-float s[2][2],y[2];
+		s[0][0]=R_A;
-//y=z-H*x, s=p*H'
+		s[1][0]=R_C;
-if(Heading_flag)
+		if(Heading_flag)
-{
+		{
-	y[0]=Heading-x[0];
+			y[0]=Heading-x[0];
-	s[0][0]=p[0][0]+R_A;
+			s[0][0]+=p[0][0];
-	s[1][0]=p[1][0]+R_C;
+			s[1][0]+=p[1][0];
-	s[0][1]=p[0][1]+R_B;
+		}
-}
+		else
-else
+		{
-{
+			y[0]=0;
-	y[0]=0;
+		}
-	s[0][0]=R_A;
+		y[1]=rate-x[1];
-	s[1][0]=R_C;
+		s[0][1]=p[0][1]+R_B;
-	s[0][1]=R_B;
+		s[1][1]=p[1][1]+R_D;
-}
+		//s=H'*s^-1
-y[1]=rate-x[1];
+		float b=1.0/(s[0][0]*s[1][1]-s[0][1]*s[1][0]);
-s[1][1]=p[1][1]+R_D;
+		if(Heading_flag)
-//s=H'*s^-1
+		{
-b=1.0/(s[0][0]*s[1][1]-s[0][1]*s[1][0]);
+			a=b*s[1][1];
-if(Heading_flag)
+			s[1][1]=b*s[0][0];
-{
+			s[0][0]=a;
-	a=b*s[1][1];
+			s[0][1]=-b*s[0][1];
-	s[1][1]=b*s[0][0];
+		}
-	s[0][0]=a;
+		else
-	s[0][1]=-b*s[0][1];
+		{
-}
+			s[1][1]=b*s[0][0];
-else
+			s[0][1]=0;
-{
+			s[0][0]=0;
-	s[1][1]=b*s[0][0];
+		}
-	s[0][1]=0;
+		s[1][0]=-b*s[1][0];
-	s[0][0]=0;
+		//s=p*s
-}
+		a=p[0][0]*s[0][0]+p[0][1]*s[1][0];
-s[1][0]=-b*s[1][0];
+		s[1][0]=p[1][0]*s[0][0]+p[1][1]*s[1][0];
-//s=p*s
+		s[0][0]=a;
-a=p[0][0]*s[0][0]+p[0][1]*s[1][0];
+		a=p[0][0]*s[0][1]+p[0][1]*s[1][1];
-s[1][0]=p[1][0]*s[0][0]+p[1][1]*s[1][0];
+		s[1][1]=p[1][0]*s[0][1]+p[1][1]*s[1][1];
-s[0][0]=a;
+		s[0][1]=a;
-a=p[0][0]*s[0][1]+p[0][1]*s[1][1];
+		//x=x+s*y
-s[1][1]=p[1][0]*s[0][1]+p[1][1]*s[1][1];
+		x[0]+=s[0][0]*y[0]+s[0][1]*y[1];
-s[1][0]=a;
+		x[1]+=s[1][0]*y[0]+s[1][1]*y[1];
-//x=x+s*y
+		//free(&y);
-x[0]+=s[0][0]*y[0]+s[0][1]*y[1];
+		//s=s*H*p
-x[1]+=s[1][0]*y[0]+s[1][1]*y[1];
+		if(Heading_flag)
-//free(&y);
+		{
-//s=sHp
+			s[0][0]=s[0][1]*p[1][0]+s[0][0]*p[0][0];
-s[0][0]=s[0][1]*p[1][0];
+			s[0][1]=s[0][1]*p[1][1]+s[0][1]*p[0][1];
-s[0][1]=s[0][1]*p[1][1];
+			s[1][0]=s[1][1]*p[1][0]+s[1][0]*p[0][0];
-s[1][0]=s[1][1]*p[1][0];
+			s[1][1]=s[1][1]*p[1][1]+s[1][0]*p[0][1];
-s[1][1]=s[1][1]*p[1][1];
+		}
-if(Heading_flag)
+		else
-{
+		{
-	s[0][0]+=s[0][0]*p[0][0];
+			s[0][0]=s[0][1]*p[1][0];
-	s[0][1]+=s[0][1]*p[0][1];
+			s[0][1]=s[0][1]*p[1][1];
-	s[1][0]+=s[1][0]*p[0][0];
+			s[1][0]=s[1][1]*p[1][0];
-	s[1][1]+=s[1][0]*p[0][1];
+			s[1][1]=s[1][1]*p[1][1];
-}
+		}
-//p=p-s
+		//p=p-s
-p[0][0]-=s[0][0];
+		p[0][0]-=s[0][0];
-p[0][1]-=s[0][1];
+		p[0][1]-=s[0][1];
-p[1][0]-=s[1][0];
+		p[1][0]-=s[1][0];
-p[1][1]-=s[1][1];
+		p[1][1]-=s[1][1];
-//free(&s);
-//free(&a);
-//free(&b);
 </code>