C语言编写卡尔曼滤波算法的C语言程序 大家自行移植到各平台

1. /*  kalman.c
   
2. 
   
3.     This file contains the code for a kalman filter, an
   
4.     extended kalman filter, and an iterated extended kalman filter.
   
5. 
   
6.     For ready extensibility, the apply_measurement() and apply_system()
   
7.     functions are located in a separate file: kalman_cam.c is an example.
   
8. 
   
9.     It uses the matmath functions provided in an accompanying file
   
10.     to perform matrix and quaternion manipulation.
    
11. 
    
12. 
    
13.     J. Watlington, 11/15/95
    
14. 
    
15.     Modified:
    
16.     11/30/95  wad  The extended kalman filter section seems to be
    
17.                    working now.
    
18. */
    
19. 
    
20. #include <stdio.h>
    
21. #include <stdlib.h>
    
22. #include <math.h>
    
23. #include "kalman.h"
    
24. 
    
25. #define ITERATION_THRESHOLD      2.0
    
26. #define ITERATION_DIVERGENCE     20
    
27. 
    
28. /*  The following are the global variables of the Kalman filters,
    
29.     used to point to data structures used throughout.     */
    
30. 
    
31. static m_elem  *state_pre;         /* ptr to apriori state vectors, x(-)     */
    
32. static m_elem  *state_post;        /* ptr to aposteriori state vectors, x(+) */
    
33. 
    
34. static m_elem  *iter_state0;
    
35. static m_elem  *iter_state1;
    
36. 
    
37. static m_elem  **cov_pre;          /* ptr to apriori covariance matrix, P(-) */
    
38. static m_elem  **cov_post;         /* ptr to apriori covariance matrix, P(-) */
    
39. static m_elem  **sys_noise_cov;    /* system noise covariance matrix (GQGt)  */
    
40. static m_elem  **mea_noise_cov;    /* measurement noise variance vector (R)  */
    
41. 
    
42. static m_elem  **sys_transfer;     /* system transfer function (Phi)    */
    
43. static m_elem  **mea_transfer;     /* measurement transfer function (H) */
    
44. 
    
45. static m_elem  **kalman_gain;      /* The Kalman Gain matrix (K) */
    
46. 
    
47. int            global_step = 0;    /* the current step number (k) */
    
48. int            measurement_size;   /* number of elems in measurement */
    
49. int            state_size;         /* number of elements in state    */
    
50. 
    
51. /*  Temporary variables, declared statically to avoid lots of run-time
    
52.     memory allocation.      */
    
53. 
    
54. static m_elem  *z_estimate;        /* a measurement_size x 1 vector */
    
55. static m_elem  **temp_state_state; /* a state_size x state_size matrix */
    
56. static m_elem  **temp_meas_state;  /* a measurement_size x state_size matrix */
    
57. static m_elem  **temp_meas_meas;   /* a measurement_size squared matrix */
    
58. static m_elem  **temp_meas_2;      /* another one ! */
    
59. 
    
60. /*  Prototypes of internal functions  */
    
61. 
    
62. static void alloc_globals( int num_state,
    
63.                           int num_measurement );
    
64. static void update_system( m_elem *z, m_elem *x_minus,
    
65.                          m_elem **kalman_gain, m_elem *x_plus );
    
66. static void estimate_prob( m_elem **P_post, m_elem **Phi, m_elem **GQGt,
    
67.                           m_elem **P_pre );
    
68. static void update_prob( m_elem **P_pre, m_elem **R, m_elem **H,
    
69.                         m_elem **P_post, m_elem **K );
    
70. static void take_inverse( m_elem **in, m_elem **out, int n );
    
71. static m_elem calc_state_change( m_elem *a, m_elem *b );
    
72. 
    
73. 
    
74. /******************************************************************
    
75. 
    
76.   Linear Kalman Filtering
    
77. 
    
78.   kalman_init()
    
79.   This function initializes the kalman filter.  Note that for a
    
80.   straight-forward (linear) Kalman filter, this is the only place that
    
81.   K and P are computed...      */
    
82. 
    
83. void kalman_init( m_elem **GQGt, m_elem **Phi, m_elem **H, m_elem **R,
    
84.                 m_elem **P, m_elem *x, int num_state, int num_measurement )
    
85. {
    
86.   alloc_globals( num_state, num_measurement );
    
87. 
    
88.   /*  Init the global variables using the arguments.  */
    
89. 
    
90.   vec_copy( x, state_post, state_size );
    
91.   mat_copy( P, cov_post, state_size, state_size );
    
92. 
    
93.   sys_noise_cov = GQGt;
    
94.   mea_noise_cov = R;
    
95. 
    
96.   sys_transfer = Phi;
    
97.   mea_transfer = H;
    
98. 
    
99.   /*****************  Gain Loop  *****************/
    
100. 
     
101.   estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
     
102.   update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );
     
103. }
     
104. 
     
105. 
     
106. /*  kalman_step()
     
107.     This function takes a set of measurements, and performs a single
     
108.     recursion of the straight-forward kalman filter.
     
109. */
     
110. 
     
111. void kalman_step( m_elem *z_in )
     
112. {
     
113.   /**************  Estimation Loop  ***************/
     
114. 
     
115.   apply_system( state_post, state_pre );
     
116.   update_system( z_in, state_pre, kalman_gain, state_post );
     
117. 
     
118.   global_step++;
     
119. }
     
120. 
     
121. /*  kalman_get_state
     
122.     This function returns a pointer to the current estimate (a posteriori)
     
123.     of the system state.         */
     
124. 
     
125. m_elem *kalman_get_state( void )
     
126. {
     
127.   return( state_post );
     
128. }
     
129. 
     
130. /******************************************************************
     
131. 
     
132.   Non-linear Kalman Filtering
     
133. 
     
134.   extended_kalman_init()
     
135.   This function initializes the extended kalman filter.
     
136. */
     
137. 
     
138. void extended_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x,
     
139.                           int num_state, int num_measurement )
     
140. {
     
141. #ifdef PRINT_DEBUG
     
142.   printf( "ekf: Initializing filter\n" );
     
143. #endif
     
144. 
     
145.   alloc_globals( num_state, num_measurement );
     
146. 
     
147.   sys_transfer = matrix( 1, num_state, 1, num_state );
     
148.   mea_transfer = matrix( 1, num_measurement, 1, num_state );
     
149. 
     
150.   /*  Init the global variables using the arguments.  */
     
151. 
     
152.   vec_copy( x, state_post, state_size );
     
153.   vec_copy( x, state_pre, state_size );
     
154.   mat_copy( P, cov_post, state_size, state_size );
     
155.   mat_copy( P, cov_pre, state_size, state_size );
     
156. 
     
157.   sys_noise_cov = GQGt;
     
158.   mea_noise_cov = R;
     
159. }
     
160. 
     
161. 
     
162. /*  extended_kalman_step()
     
163.     This function takes a set of measurements, and performs a single
     
164.     recursion of the extended kalman filter.
     
165. */
     
166. 
     
167. void extended_kalman_step( m_elem *z_in )
     
168. {
     
169. #ifdef PRINT_DEBUG
     
170.   printf( "ekf: step %d\n", global_step );
     
171. #endif
     
172.   /*****************  Gain Loop  *****************
     
173.     First, linearize locally, then do normal gain loop    */
     
174. 
     
175.   generate_system_transfer( state_pre, sys_transfer );
     
176.   generate_measurement_transfer( state_pre, mea_transfer );
     
177. 
     
178.   estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
     
179.   update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );
     
180. 
     
181.   /**************  Estimation Loop  ***************/
     
182. 
     
183.   apply_system( state_post, state_pre );
     
184.   update_system( z_in, state_pre, kalman_gain, state_post );
     
185. 
     
186.   global_step++;
     
187. }
     
188. 
     
189. 
     
190. /* iter_ext_kalman_init()
     
191.    This function initializes the iterated extended kalman filter
     
192. */
     
193. 
     
194. void iter_ext_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x,
     
195.                           int num_state, int num_measurement )
     
196. {
     
197. #ifdef PRINT_DEBUG
     
198.   printf( "iekf: Initializing filter\n" );
     
199. #endif
     
200. 
     
201.   alloc_globals( num_state, num_measurement );
     
202. 
     
203.   iter_state0  = vector( 1, num_state );
     
204.   iter_state1  = vector( 1, num_state );
     
205.   sys_transfer = matrix( 1, num_state, 1, num_state );
     
206.   mea_transfer = matrix( 1, num_measurement, 1, num_state );
     
207. 
     
208.   /*  Init the global variables using the arguments.  */
     
209. 
     
210.   vec_copy( x, state_post, state_size );
     
211.   vec_copy( x, state_pre, state_size );
     
212.   mat_copy( P, cov_post, state_size, state_size );
     
213.   mat_copy( P, cov_pre, state_size, state_size );
     
214. 
     
215.   sys_noise_cov = GQGt;
     
216.   mea_noise_cov = R;
     
217. }
     
218. 
     
219. /*  iter_ext_kalman_step()
     
220.     This function takes a set of measurements, and iterates over a single
     
221.     recursion of the extended kalman filter.
     
222. */
     
223. 
     
224. void iter_ext_kalman_step( m_elem *z_in )
     
225. {
     
226.   int     iteration = 1;
     
227.   m_elem  est_change;
     
228.   m_elem  *prev_state;
     
229.   m_elem  *new_state;
     
230.   m_elem  *temp;
     
231. 
     
232.   generate_system_transfer( state_pre, sys_transfer );
     
233.   estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
     
234.   apply_system( state_post, state_pre );
     
235. 
     
236.   /*  Now iterate, updating the probability and the system model
     
237.       until no change is noticed between iteration steps      */
     
238. 
     
239.   prev_state = iter_state0;
     
240.   new_state  = iter_state1;
     
241. 
     
242.   generate_measurement_transfer( state_pre, mea_transfer );
     
243.   update_prob( cov_pre, mea_noise_cov, mea_transfer,
     
244.               cov_post, kalman_gain );
     
245.   update_system( z_in, state_pre, kalman_gain, prev_state );
     
246.   est_change = calc_state_change( state_pre, prev_state );
     
247. 
     
248.   while( (est_change < ITERATION_THRESHOLD) &&
     
249. 
     

复制代码

全部代码51hei下载地址：
C语言卡尔曼滤波.zip (11.45 KB, 下载次数: 56)

2021-6-30 14:39 上传

点击文件名下载附件

学习学习啦！！效果不知道如何呢！收藏啦~~~

好东西 用在哪里？

好东西，学习了，有没有c语言写的低通，带通滤波器呢