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 21:46] – laurencebcode:kalman_filter [2008/10/20 13:38] (current) – laurenceb
Line 18: 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 43: 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 59: 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 90: Line 101:
 { {
  vector state; //system state  vector state; //system state
- matrix P; //estimate covariance+ matrix P; //estimate covariance
 } kalman_state; } kalman_state;
  
Line 102: 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 146: Line 156:
  return ans;  return ans;
 } }
 +/*
 matrix subtract(matrix * a, matrix * b) matrix subtract(matrix * a, matrix * b)
 { {
Line 156: Line 166:
  return ans;  return ans;
 } }
 +*/
 matrix transpose(matrix * a) matrix transpose(matrix * a)
 { {
Line 190: Line 200:
  return ans;  return ans;
 } }
 +
 </code> </code>
  
code/kalman_filter.1208295995.txt.gz · Last modified: 2008/07/19 23:31 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki