UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools

Trace:
code:optimised_kalman

Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
code:optimised_kalman [2008/11/30 05:15] – laurencebcode: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> <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
  float s[2][2];  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][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[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][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;  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);  memcpy(p,s,16);
- 
- //free(p_); 
- 
  //update  //update
- 
  float y[2];  float y[2];
- 
  //y=z-H*x, s=p*H'+R  //y=z-H*x, s=p*H'+R
- 
  s[0][0]=R_A;  s[0][0]=R_A;
- 
  s[1][0]=R_C;  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];  s[0][0]+=p[0][0];
- 
  s[1][0]+=p[1][0];  s[1][0]+=p[1][0];
- 
  }  }
- 
  else  else
- 
  {  {
- 
  y[0]=0;  y[0]=0;
- 
  }  }
- 
  y[1]=rate-x[1];  y[1]=rate-x[1];
  s[0][1]=p[0][1]+R_B;  s[0][1]=p[0][1]+R_B;
- 
  s[1][1]=p[1][1]+R_D;  s[1][1]=p[1][1]+R_D;
- 
  //s=H'*s^-1  //s=H'*s^-1
- 
  float b=1.0/(s[0][0]*s[1][1]-s[0][1]*s[1][0]);  float b=1.0/(s[0][0]*s[1][1]-s[0][1]*s[1][0]);
- 
  if(Heading_flag)  if(Heading_flag)
- 
  {  {
- 
  a=b*s[1][1];  a=b*s[1][1];
- 
  s[1][1]=b*s[0][0];  s[1][1]=b*s[0][0];
- 
  s[0][0]=a;  s[0][0]=a;
- 
  s[0][1]=-b*s[0][1];  s[0][1]=-b*s[0][1];
- 
  }  }
- 
  else  else
- 
  {  {
- 
  s[1][1]=b*s[0][0];  s[1][1]=b*s[0][0];
- 
  s[0][1]=0;  s[0][1]=0;
- 
  s[0][0]=0;  s[0][0]=0;
- 
  }  }
- 
  s[1][0]=-b*s[1][0];  s[1][0]=-b*s[1][0];
- 
  //s=p*s  //s=p*s
- 
  a=p[0][0]*s[0][0]+p[0][1]*s[1][0];  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[1][0]=p[1][0]*s[0][0]+p[1][1]*s[1][0];
- 
  s[0][0]=a;  s[0][0]=a;
- 
  a=p[0][0]*s[0][1]+p[0][1]*s[1][1];  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][1]=p[1][0]*s[0][1]+p[1][1]*s[1][1];
- 
  s[0][1]=a;  s[0][1]=a;
- 
  //x=x+s*y  //x=x+s*y
- 
  x[0]+=s[0][0]*y[0]+s[0][1]*y[1];  x[0]+=s[0][0]*y[0]+s[0][1]*y[1];
- 
  x[1]+=s[1][0]*y[0]+s[1][1]*y[1];  x[1]+=s[1][0]*y[0]+s[1][1]*y[1];
- 
  //free(&y);  //free(&y);
- + //s=s*H*p
- //s=sHp +
  if(Heading_flag)  if(Heading_flag)
- 
  {  {
- 
  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][0]*p[0][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[0][1]*p[0][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][0]*p[0][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]+s[1][0]*p[0][1];
- 
  }  }
  else  else
  {  {
  s[0][0]=s[0][1]*p[1][0];  s[0][0]=s[0][1]*p[1][0];
- 
  s[0][1]=s[0][1]*p[1][1];  s[0][1]=s[0][1]*p[1][1];
- 
  s[1][0]=s[1][1]*p[1][0];  s[1][0]=s[1][1]*p[1][0];
- 
  s[1][1]=s[1][1]*p[1][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];
 </code> </code>
code/optimised_kalman.1228022118.txt.gz · Last modified: 2008/11/30 05:15 by laurenceb
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki