"Fossies" - the Fresh Open Source Software Archive 
Member "HTTPing-2.9/kalman.c" (29 Oct 2022, 1922 Bytes) of package /linux/www/HTTPing-2.9.tar.gz:
As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style:
standard) with prefixed line numbers and
code folding option.
Alternatively you can here
view or
download the uninterpreted source code file.
For more information about "kalman.c" see the
Fossies "Dox" file reference documentation.
1 /* #define _TEST */
2
3 #include <math.h>
4 #ifdef _TEST
5 #include <stdio.h>
6 #include <stdlib.h>
7 #endif
8
9 double x_est_last = 0.0, P_last = 0.0, Q = 0.0, R = 0.0, K = 0.0, P = 0.0, P_temp = 0.0, x_temp_est = 0.0, x_est = 0.0, z_measured = 0.0, z_real = 0.0, sum_error_kalman = 0.0, sum_error_measure = 0.0;
10 char first = 1;
11
12 void kalman_init(double ideal_value)
13 {
14 /* initial values for the kalman filter */
15 x_est_last = 0;
16 P_last = 0;
17 /* the noise in the system (FIXME?) */
18 Q = 0.022;
19 R = 0.617;
20 z_real = ideal_value; /* 0.5; the ideal value we wish to measure */
21
22 first = 1;
23 }
24
25 double kalman_do(double z_measured)
26 {
27 /* initialize with a measurement */
28 if (first)
29 {
30 first = 0;
31 x_est_last = z_measured;
32 }
33
34 /* do a prediction */
35 x_temp_est = x_est_last;
36 P_temp = P_last + Q;
37 /* calculate the Kalman gain */
38 K = P_temp * (1.0/(P_temp + R));
39 /* measure */
40 /*z_measured = z_real + frand()*0.09; the real measurement plus noise*/
41 /* correct */
42 x_est = x_temp_est + K * (z_measured - x_temp_est);
43 P = (1- K) * P_temp;
44 /* we have our new system */
45
46 #ifdef _TEST
47 printf("Ideal position: %6.3f \n",z_real);
48 printf("Measured position: %6.3f [diff:%.3f]\n",z_measured,fabs(z_real-z_measured));
49 printf("Kalman position: %6.3f [diff:%.3f]\n",x_est,fabs(z_real - x_est));
50 #endif
51
52 sum_error_kalman += fabs(z_real - x_est);
53 sum_error_measure += fabs(z_real-z_measured);
54
55 /* update our last's */
56 P_last = P;
57 x_est_last = x_est;
58
59 #ifdef _TEST
60 printf("Total error if using raw measured: %f\n",sum_error_measure);
61 printf("Total error if using kalman filter: %f\n",sum_error_kalman);
62 printf("Reduction in error: %d%% \n",100-(int)((sum_error_kalman/sum_error_measure)*100));
63 #endif
64
65 return x_est;
66 }
67
68 #ifdef _TEST
69 int main(int argc, char *argv[])
70 {
71 kalman_init(0.0);
72
73 for(int loop=0; loop<25; loop++)
74 {
75 double v = drand48();
76 printf("%d] %f %f\n", loop + 1, v, kalman_do(v));
77 }
78
79 return 0;
80 }
81 #endif