Differences

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

--- code:optimised_kalman [2008/11/30 00:50] – created 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>
-		//kalman_state our_state_;
+		//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
 		//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];
-		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]);
+		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;
-		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[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;
-		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]);
+		memcpy(p,s,16);
-		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 y[2];
-		float s[2][2],y[2];
+		//y=z-H*x, s=p*H'+R
+		s[0][0]=R_A;
-		//y=z-H*x, s=p*H'
+		s[1][0]=R_C;
 		if(Heading_flag)
 		{
 			y[0]=Heading-x[0];
+			s[0][0]+=p[0][0];
-			s[0][0]=p[0][0]+R_A;
+			s[1][0]+=p[1][0];
-			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[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]);
-		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
-		//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[0][1]=a;
-		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=s*H*p
-		//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][1]*p[1][0]+s[0][0]*p[0][0];
-			s[0][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[0][1]+=s[0][1]*p[0][1];
+			s[1][1]=s[1][1]*p[1][1]+s[1][0]*p[0][1];
-			s[1][0]+=s[1][0]*p[0][0];
-			s[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];
-		//free(&s);
-		//free(&a);
-		//free(&b);
 </code>