UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools

Trace:
code:kalman_filter

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:kalman_filter [2008/04/13 14:30] – laurencebcode:kalman_filter [2008/10/20 13:38] (current) – laurenceb
Line 1: Line 1:
 +
 +This is a two state Kalman filter designed for combining rate gyroscope and GPS heading data for use on guided parafoils and rogallo wings. The "predict and update" routine uses 18500 clock cycles, so can run in under a millisecond.
 +
 +===== Testing =====
 +
 + {{code:screenshot-gnuplot-3.png?800|}}
 +
 + {{code:screenshot-gnuplot-4.png?800|}}
 +
 + Test with data for an oscillating parafoil (hopefully won't happen). Green is the filter heading output, pink GPS heading, red gyro output, and blue filtered rate output. Time is in units of 20ms, so ticks are at 20 second intervals.
 +
 +{{code:screenshot-gnuplot-1.png?750|}}
 +
 +
 + The simulated gps heading is shown in blue, with the kalman filter output for heading in red, and turn rate in green.
 +
 +{{code:screenshot-gnuplot-2.png?750|}}
 +
 + The simulated gyro output in red, and filter turn rate output in green.
 +
 +
 +
 +
 +
 +
 ===== Kalman.c ===== ===== Kalman.c =====
  
-<code c> +<code c>#include "kalman.h" 
-#include "main.h"+#include "matrix.h"
  
  
-kalman_state predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y) +void predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y) 
 { {
- kalman_state New; + predict_state(model,Prev,u); 
- matrix K; + predict_P(model,Prev); 
- New.state=predict_state(model,Prev,u); + matrix K=optimal_gain(model,Prev); 
- New.P=predict_P(model,Prev); + state_update(Prev,model,&K,y); 
- K=optimal_gain(model,Prev); + P_update(&K,model,Prev);
- New.state=state_update(Prev,model,&K,y); +
- New.P=P_update(&K,model,Prev); +
- return New;+
 } }
  
-vector predict_state(kalman_model * model, kalman_state * state, vector * u) //x is system state, u control input(s), B input model+void predict_state(kalman_model * model, kalman_state * state, vector * u) //x is system state, u control input(s), B input model
 { {
  vector a;  vector a;
Line 23: Line 45:
  a=matrix_vector(&model->F,&state->state);  a=matrix_vector(&model->F,&state->state);
  b=matrix_vector(&model->B,u);  b=matrix_vector(&model->B,u);
- return vector_add(&a,&b); //estimated state+ state->state = vector_add(&a,&b); //estimated state
 } }
  
-matrix predict_P(kalman_model * model, kalman_state * state) //F is propagation model, Q model noise+void predict_P(kalman_model * model, kalman_state * state) //F is propagation model, Q model noise
 { {
  matrix S=transpose(&model->F);  matrix S=transpose(&model->F);
  S=multiply(&state->P,&S);  S=multiply(&state->P,&S);
  S=multiply(&model->F,&S);  S=multiply(&model->F,&S);
- return add(&S,&model->Q); //estimated estimate covariance+ state->P = add(&S,&model->Q); //estimated estimate covariance
 } }
  
Line 39: Line 61:
  matrix S=multiply(&state->P,&T);  matrix S=multiply(&state->P,&T);
  S=multiply(&model->H,&S);  S=multiply(&model->H,&S);
- S=add(&model->R,&S); //S is now the innovation covariance + S=add(&model->R,&S); //S is now the innovation covariance
  S=inverse(&S);  S=inverse(&S);
  S=multiply(&T,&S);  S=multiply(&T,&S);
- return multiply(&state->P,&S); //the optimal gain+ return multiply(&state->P,&S); //the optimal gain
 }  } 
  
-vector state_update(kalman_state * state, kalman_model * model, matrix * K, vector * y) //y is the measurements+void state_update(kalman_state * state, kalman_model * model, matrix * K, vector * y) //y is the measurements
 { {
  vector a=matrix_vector(&model->H,&state->state);  vector a=matrix_vector(&model->H,&state->state);
- a=vector_subtract(y,&a); //a is now the difference or residual + a=vector_subtract(y,&a); //a is now the difference or residual 
- matrix_vector(K,&a); + if(a.t>PI) //need to make sure we stay in the correct range 
- return vector_add(&state->state,&a);+ { 
 + a.t-=2*PI; 
 + } 
 + if(a.t<PI) 
 + { 
 + a.t+=2*PI; 
 + } 
 + a=matrix_vector(K,&a); 
 + state->state = vector_add(&state->state,&a);
 } }
  
-matrix P_update(matrix *K, kalman_model * model, kalman_state * state) //K is optimal gain, H measurement model+void P_update(matrix *K, kalman_model * model, kalman_state * state) //K is optimal gain, H measurement model
 { {
  matrix S=multiply(K,&model->H);  matrix S=multiply(K,&model->H);
- S.tl=1-S.tl; //subtraction from identity+ S.tl=1.0-S.tl; //subtraction from identity
  S.tr=-S.tr;  S.tr=-S.tr;
  S.bl=-S.bl;  S.bl=-S.bl;
- S.br=1-S.br; + S.br=1.0-S.br; 
- return multiply(&S,&state->P);+ state->P = multiply(&S,&state->P);
 } }
 </code> </code>
 +
  
 ===== Kalman.h ===== ===== Kalman.h =====
Line 70: Line 101:
 { {
  vector state; //system state  vector state; //system state
- matrix P; //estimate covariance+ matrix P; //estimate covariance
 } kalman_state; } kalman_state;
  
Line 82: Line 113:
 } kalman_model; } kalman_model;
  
-kalman_state predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y); +void predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y); 
-vector predict_state(kalman_model * model, kalman_state * state, vector * u); +void predict_state(kalman_model * model, kalman_state * state, vector * u); 
-matrix predict_P(kalman_model * model, kalman_state * state);+void predict_P(kalman_model * model, kalman_state * state);
 matrix optimal_gain(kalman_model * model, kalman_state * state); matrix optimal_gain(kalman_model * model, kalman_state * state);
-vector state_update(kalman_state * state, matrix * K, vector * y); +void state_update(kalman_state * state, matrix * K, vector * y); 
-matrix P_update(matrix *K, kalman_model * model, kalman_state * state);+void P_update(matrix *K, kalman_model * model, kalman_state * state);
 </code> </code>
  
 ===== Matrix.c ===== ===== Matrix.c =====
-<code c> +<code c>#include "matrix.h"
-#include "main.h"+
 //matrix operations //matrix operations
  
Line 126: Line 156:
  return ans;  return ans;
 } }
 +/*
 matrix subtract(matrix * a, matrix * b) matrix subtract(matrix * a, matrix * b)
 { {
Line 136: Line 166:
  return ans;  return ans;
 } }
 +*/
 matrix transpose(matrix * a) matrix transpose(matrix * a)
 { {
Line 170: Line 200:
  return ans;  return ans;
 } }
 +
 </code> </code>
  
code/kalman_filter.1208097006.txt.gz · Last modified: 2008/07/19 23:31 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki