├── .gitignore
├── LICENSE
├── README.md
├── scalar_kalman.c
└── scalar_kalman.h


/.gitignore:
--------------------------------------------------------------------------------
 1 | # Object files
 2 | *.o
 3 | *.ko
 4 | *.obj
 5 | *.elf
 6 | 
 7 | # Precompiled Headers
 8 | *.gch
 9 | *.pch
10 | 
11 | # Libraries
12 | *.lib
13 | *.a
14 | *.la
15 | *.lo
16 | 
17 | # Shared objects (inc. Windows DLLs)
18 | *.dll
19 | *.so
20 | *.so.*
21 | *.dylib
22 | 
23 | # Executables
24 | *.exe
25 | *.out
26 | *.app
27 | *.i*86
28 | *.x86_64
29 | *.hex
30 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | The MIT License (MIT)
 2 | 
 3 | Copyright (c) 2015 electron
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 
23 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | scalar_kalman_filter
2 | ====================
3 | 
4 | 标量Kalman滤波器的C语言实现<br>
5 | C implementation of Scalar Kalman Filter
6 | 


--------------------------------------------------------------------------------
/scalar_kalman.c:
--------------------------------------------------------------------------------
 1 | #include "scalar_kalman.h"
 2 | 
 3 | /**
 4 | * 	状态方程: x = A*x + w; w~N(0,Q)
 5 | * 	测量方程: y = C*x + v; v~N(0,R)
 6 |  */
 7 | 
 8 | float scalar_kalman(scalar_kalman_t *kalman, float y)
 9 | {
10 | 	// 状态预测
11 | 	kalman->x = kalman->A * kalman->x;
12 | 	// 误差协方差预测
13 | 	kalman->P = kalman->A2 * kalman->P + kalman->Q;
14 | 	// 计算卡尔曼滤波增益
15 | 	kalman->K = kalman->P * kalman->C / (kalman->C2 * kalman->P + kalman->R);
16 | 	
17 | 	// 状态估计校正
18 | 	kalman->x = kalman->x + kalman->K * (y - kalman->C * kalman->x);
19 | 	// 误差协方差估计校正
20 | 	kalman->P = (1 - kalman->K * kalman->C) * kalman->P;
21 | 	
22 | 	return kalman->C * kalman->x; 	// 输出滤波后的y
23 | }
24 | 
25 | void scalar_kalman_init(scalar_kalman_t *kalman, float A, float C, float Q, float R)
26 | {
27 | 	kalman->A = A;
28 | 	kalman->A2 = A * A;
29 | 	kalman->C = C;
30 | 	kalman->C2 = C * C;
31 | 
32 | 	kalman->Q = Q;
33 | 	kalman->R = R;
34 | 
35 | 	kalman->x = 0;
36 | 	kalman->P = Q;
37 | 	kalman->K = 1;
38 | }
39 | 


--------------------------------------------------------------------------------
/scalar_kalman.h:
--------------------------------------------------------------------------------
 1 | #ifndef SCALAR_KALMAN_H
 2 | #define SCALAR_KALMAN_H
 3 | 
 4 | /**
 5 |  * 	x = A*x + w; w~N(0,Q)
 6 |  * 	y = C*x + v; v~N(0,R)
 7 |  */
 8 | typedef struct scalar_kalman_s
 9 | {
10 | 	float A, C;
11 | 	float A2, C2; 	// A,C 的平方
12 | 
13 | 	float Q, R;
14 | 	float K, P;
15 | 
16 | 	float x;
17 | }scalar_kalman_t;
18 | 
19 | void scalar_kalman_init(scalar_kalman_t *kalman, float A, float C, float Q, float R);
20 | float scalar_kalman(scalar_kalman_t *kalman, float y);
21 | 
22 | #endif


--------------------------------------------------------------------------------