Created
April 3, 2014 09:53
-
-
Save jannson/9951716 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/** A simple kalman filter example by Adrian Boeing | |
www.adrianboeing.com | |
*/ | |
#include <stdio.h> | |
#include <stdlib.h> | |
#include <math.h> | |
double frand() { | |
return 2*((rand()/(double)RAND_MAX) - 0.5); | |
} | |
int main() { | |
//initial values for the kalman filter | |
float x_est_last = 0; | |
float P_last = 0; | |
//the noise in the system | |
float Q = 0.022; | |
float R = 0.617; | |
float K; | |
float P; | |
float P_temp; | |
float x_temp_est; | |
float x_est; | |
float z_measured; //the 'noisy' value we measured | |
float z_real = 0.5; //the ideal value we wish to measure | |
srand(0); | |
//initialize with a measurement | |
x_est_last = z_real + frand()*0.09; | |
float sum_error_kalman = 0; | |
float sum_error_measure = 0; | |
for (int i=0;i<30;i++) { | |
//do a prediction | |
x_temp_est = x_est_last; | |
P_temp = P_last + Q; | |
//calculate the Kalman gain | |
K = P_temp * (1.0/(P_temp + R)); | |
//measure | |
z_measured = z_real + frand()*0.09; //the real measurement plus noise | |
//correct | |
x_est = x_temp_est + K * (z_measured - x_temp_est); | |
P = (1- K) * P_temp; | |
//we have our new system | |
printf("Ideal position: %6.3f \n",z_real); | |
printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measured,fabs(z_real-z_measured)); | |
printf("Kalman position: %6.3f [diff:%.3f]\n",x_est,fabs(z_real - x_est)); | |
sum_error_kalman += fabs(z_real - x_est); | |
sum_error_measure += fabs(z_real-z_measured); | |
//update our last's | |
P_last = P; | |
x_est_last = x_est; | |
} | |
printf("Total error if using raw measured: %f\n",sum_error_measure); | |
printf("Total error if using kalman filter: %f\n",sum_error_kalman); | |
printf("Reduction in error: %d%% \n",100-(int)((sum_error_kalman/sum_error_measure)*100)); | |
return 0; | |
} |
After some iterations K factor becomes constant . why are we calculating K factor again and again .
Where from you got these values of float Q = 0.022; R = 0.617;
After some iterations K factor becomes constant . why are we calculating K factor again and again .
For Uncertainty that may occurs in dynamic of system and measuring.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
How to find the noise in the input signal for a system with dynamic behaviour?
I dont get how values of float Q = 0.022; R = 0.617; were designated.