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/15 20:31] – laurencebcode:kalman_filter [2008/10/20 13:38] (current) – laurenceb
Line 3: Line 3:
  
 ===== Testing ===== ===== 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|}} {{code:screenshot-gnuplot-1.png?750|}}
Line 12: Line 18:
  
  The simulated gyro output in red, and filter turn rate output in green.  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 37: 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 53: 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 84: Line 101:
 { {
  vector state; //system state  vector state; //system state
- matrix P; //estimate covariance+ matrix P; //estimate covariance
 } kalman_state; } kalman_state;
  
Line 96: 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 140: Line 156:
  return ans;  return ans;
 } }
 +/*
 matrix subtract(matrix * a, matrix * b) matrix subtract(matrix * a, matrix * b)
 { {
Line 150: Line 166:
  return ans;  return ans;
 } }
 +*/
 matrix transpose(matrix * a) matrix transpose(matrix * a)
 { {
Line 184: Line 200:
  return ans;  return ans;
 } }
 +
 </code> </code>
  
code/kalman_filter.1208291469.txt.gz · Last modified: 2008/07/19 23:31 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki