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/09/10 22:28] – 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.
 +
 +
 +
  
  
Line 28: Line 31:
  
 void 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) 
- 
 { {
- 
  predict_state(model,Prev,u);  predict_state(model,Prev,u);
  predict_P(model,Prev);  predict_P(model,Prev);
- 
  matrix K=optimal_gain(model,Prev);  matrix K=optimal_gain(model,Prev);
  state_update(Prev,model,&K,y);  state_update(Prev,model,&K,y);
- 
  P_update(&K,model,Prev);  P_update(&K,model,Prev);
 } }
- 
- 
  
 void 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;
- 
  vector b;  vector b;
- 
  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);
- 
  state->state = vector_add(&a,&b); //estimated state  state->state = vector_add(&a,&b); //estimated state
- 
 } }
- 
- 
  
 void 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);
- 
  state->P = add(&S,&model->Q); //estimated estimate covariance  state->P = add(&S,&model->Q); //estimated estimate covariance
- 
 } }
- 
- 
  
 matrix optimal_gain(kalman_model * model, kalman_state * state) //H is measurement model, R measurement noise matrix optimal_gain(kalman_model * model, kalman_state * state) //H is measurement model, R measurement noise
- 
 { {
- 
  matrix T=transpose(&model->H);  matrix T=transpose(&model->H);
- 
  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
- 
 }  } 
- 
- 
  
 void 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,&state->state); //a is now the difference or residual + if(a.t>PI) //need to make sure we stay in the correct range 
 + { 
 + a.t-=2*PI; 
 + } 
 + if(a.t<PI) 
 + { 
 + a.t+=2*PI; 
 + }
  a=matrix_vector(K,&a);  a=matrix_vector(K,&a);
- 
  state->state = vector_add(&state->state,&a);  state->state = vector_add(&state->state,&a);
- 
 } }
- 
- 
  
 void 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.0-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.0-S.br;  S.br=1.0-S.br;
- 
  state->P = multiply(&S,&state->P);  state->P = multiply(&S,&state->P);
- 
 } }
 +</code>
  
-</code> 
  
 ===== Kalman.h ===== ===== Kalman.h =====
Line 152: 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>
  
code/kalman_filter.1221085731.txt.gz · Last modified: 2008/09/10 22:28 by laurenceb
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki