Bộ lọc Kalman, tác giả Nguyễn Quốc Đính
Bộ lọc Kalman là một bộ lọc đệ quy để ước lượng trạng thái của hệ thống tuyến tính, và cả hệ thống phi tuyến khi áp dụng phép ước lượng phi tuyến sang tuyến tính.
Bộ lọc Kalman được ứng dụng rất nhiều trong lĩnh vực kĩ thuật, đặc biệt là lĩnh vực điều khiển. Và dĩ nhiên lý thuyết của bộ lọc hoàn toàn không đơn giản, nhưng may mắn là có thể tìm thấy trên mạng vô số tài liệu về bộ lọc này cũng như sách, báo.
Dưới đây là bài viết tôi thực hiện khi thực tập đại học, trình bày rất đơn giản về bộ lọc Kalman, cũng như một ứng dụng nho nhỏ của bộ lọc này, nhằm mục đích loại bỏ tín hiệu nhiễu từ cảm biến. Chương trình được mô phỏng bằng Matlab.
Các giá trị đọc được từ cảm biến thường chứa nhiễu, tác động bởi nhiều nguồn khác nhau, như nhiễu quá trình, nhiễu nhiệt, nhiều từ bộ biến đổi ADC… Giá trị đọc được do đó dao đông ngay cả khi giá trị thực là hằng số. Ví dụ đơn giản như một cảm biến phao trên mặt nước của một bình chứa, gợn sóng làm giá trị thu được dao động xung quanh vị trí cân bằng ngay cả khi mực nước không đổi. Ngay lúc này bạn có thể nghĩ rằng chỉ cần lấy giá trị trung bình của các giá trị đọc về là ổn. Đúng vậy, cộng trung bình là phương pháp rất đơn giản, và khá hiệu quả. Còn Kalman là bộ lọc thông thấp bậc nhất trong trường hợp này.
Và trong trường hợp có nhiều giá trị liên quan đến đại lượng cần ước lượng được sử dụng, bộ lọc Kalman có thể kết hợp nhiều giá trị đó lại với nhau, cho ra kết quả chính xác hơn. Ví dụ như nếu biết thêm được lưu lượng nước vào và ra của bình chứa thì tổng cộng có 2 giá trị liên quan đến mực nước, là giá trị mực nước và lưu lượng vào ra, kết quả sẽ khả quan hơn với bộ lọc Kalman.
Bài viết cũng có bàn luận đến vấn đề khi giá trị mong muốn thay đổi, thì cần có sự thỏa hiệp giữa tốc độ bám đuổi của giá trị ước lượng với giá trị thực với độ dao động của giá trị ước lượng. Tức là khi muốn giá trị ước lượng thay đổi nhanh thì giá trị ước lượng không phẳng lắm, còn muốn giá trị ước lượng được phẳng hơn thì nó sẽ ít có khả năng bám theo sự thay đổi nhanh của giá trị thực tế.
Điểm màu xanh là giá trị đọc được trực tiếp từ cảm biến, đường màu đỏ là giá trị ước lượng với bộ lọc Kalman bậc 1.
Chương trình thực hiện bộ lọc bậc 1 rất đơn giản, có thể thực hiện đễ dàng trên các chíp vi điều khiển 8 bít, có thể sử dụng để xử lý các tín hiệu từ cảm biến để cho kết quả tốt hơn.
Code CCS-C lọc Kalman cho cảm biến MPU6050
Bộ lọc Kalman là thuật toán sử dụng chuỗi các giá trị đo lường, bị ảnh hưởng bởi nhiễu hoặc sai số, để ước đoán biến số nhằm tăng độ chính xác so với việc sử dụng duy nhất một giá trị đo lường. Bộ lọc Kalman thực hiện phương pháp truy hồi đối với chuỗi các giá trị đầu vào bị nhiễu, nhằm tối ưu hóa giá trị ước đoán trạng thái của hệ thống.
Bộ lọc Kalman được ứng dụng rộng rãi trong kỹ thuật, phổ biến trong các ứng dụng định hướng, định vị và điều khiển các phương tiện di chuyển. Ngoài ra, bộ lọc Kalman còn được ứng dụng để phân tích dữ liệu trong các lĩnh vực xử lý tín hiệu và kinh tế.
Lưu đồ giải thuật của bộ lọc Kalman
Code CCS-C
Đoạn chương trình lọc Kalman viết cho MCU PIC bằng trình biên dịch CCS-C
float32 dt = 0.01; // T Sampling
float32 Q_angle = 0.005;
float32 Q_bias = 0.003;
float32 R_measure = 0.03;
float32 bias = 0; // Reset bias
float32 rate;
float32 angle;
float32 S; // estimate error
float32 y; // different angle
float32 P_00 = 0 , P_01 = 0 , P_10 =0 ,P_11 =0;
float32 K_0 =0,K_1=0; // Kalman gain
float32 Kalman(float32 newAngle, float32 newRate){
// Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
//angle = X_Raw_Gyro_Angle;
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
P_00 += dt * ( dt*P_11 - P_10 - P_01 + Q_angle);
P_01 -= dt * P_11;
P_10 -= dt * P_11;
P_11 += Q_bias * dt;
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
S = P_00 + R_measure;
/* Step 5 */
K_0 = P_00 / S;
K_1 = P_10 / S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
y = newAngle - angle;
/* Step 6 */
angle += K_0 * y;
bias += K_1 * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return angle;
}
Bộ lọc Kalman – giải pháp chống nhiễu tuyệt vời cho mọi dự án sử dụng cảm biến
Trong đo lường, do các yếu tố cả chủ quan lẫn khách quan mà kết quả đo đạc luôn chỉ được coi là tương đối so với giá trị thực cần đo. Độ chênh lệch giữa giá trị đo được với giá trị thực được gọi là Sai số. Sai số gây ra bởi nhiễu, chúng được phân làm 2 loại chính:
- Sai số hệ thống
- Sai số ngẫu nhiên
Giả sử ta tiến hành đo giá trị điện áp của một cục pin năng lượng mặt trời, kết quả thu được trên đồ thị như sau:
Giả sử giá trị điện áp thực của pin mặt trời là u0=1.9545 vol.
Do ảnh hưởng bởi nhiều yếu tố trong quá trình đo đạc, sai số xuất hiện là điều rất khó tránh khỏi
Kết quả là ta chỉ có thể đo giá trị điện áp bằng cách lấy tương đối kết quả trung bình các phép đo
Tức u0=u ± ∆ e =1.95 ± 1%, (với ∆ e là sai số)
Loại bỏ nhiễu bằng thuật toán lọc Kalman
Để chứng minh hiệu quả của phương pháp này chúng ta sẽ có một phép thử mô phỏng như sau:
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
|
float u0 = 100.0; // giá trị thực (không đổi)
float e; // nhiễu
float u; // giá trị đo được (có thêm nhiễu)
void setup()
{
Serial.begin(9600);
}
void loop()
{
randomSeed(millis());
e = (float)random(–100, 100);
u = u0 + e;
Serial.println(u);
}
|
Nạp code cho arduino rồi sau đó mở cổng Serial Plotter để xem dưới dạng đồ thị:
Kết quả hiển thị trên Serial plotter:
Xem lại code bên trên ta thấy có vài điều như sau:
Gọi u0=100.0 là giá trị thực tế của vật thể, cũng là giá trị mà ta mong muốn thu được, vì u0 là hằng số (nếu như không có nhiễu).
Lý tưởng thì trên đồ thị ta sẽ thu được một đường thẳng song song với trục thời gian t.
Thường thì nhiễu chỉ dao động trong khoảng e=±10% giá trị thực đã được coi là rất nhiễu rồi (noise).
Để tăng độ khó, mình đã cố ý cho e=±100% u0 bằng hàm Random khiến cho giá trị đo bị nhiễu hoàn toàn và gần như rất khó để thu thập lẫn tính toán sau này.
Sử dụng bộ lọc Kalman
Như đã thống nhất, trong thực tế u0 là giá trị chúng ta không biết, việc sử dụng bộ lọc sẽ phải giúp ta loại bỏ các nhiễu, khi đó giá trị đo được phải gần đường u0=100 hơn .
Vì đây là mô phỏng nên giá trị u0 cần được cho trước (chỉ mình và bạn biết) để có thể kiểm chứng tính đúng đắn của kết quả trước và sau khi lọc (bằng cách trộn u0 với nhiễu rồi cho arduino lọc)
So với code bên trên, phần code này chỉ cần thêm một dòng lệnh duy nhất:
Gọi u_kalman là giá trị đo đã qua bộ lọc Kalman:
|
u_kalman=bo_loc.updateEstimate(u);
|
Code
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
#include <SimpleKalmanFilter.h>
SimpleKalmanFilter bo_loc(2, 2, 0.001);
float u0 = 100.0; // giá trị thực (không đổi)
float e; // nhiễu
float u; // giá trị đo được (có thêm nhiễu)
float u_kalman; // giá được lọc nhiễu
void setup()
{
Serial.begin(9600);
}
void loop()
{
randomSeed(millis());
e = (float)random(–100, 100);
u = u0 + e;
Serial.print(u);
Serial.print(“,”);
u_kalman = bo_loc.updateEstimate(u);
Serial.print(u_kalman);
Serial.println();
}
|
Và đây là kết quả khi sử dụng thêm bộ lọc:
Đường màu xanh: u.
Đường màu vàng: u_kalman.
Dừng lại một chút để quan sát đồ thị, hẳn bạn cũng đồng ý với mình thuật toán lọc Kalman tỏ ra rất hiệu quả, có những lúc nhiễu dồn ra biên cực đại (±100%u0) nhưng giá trị vẫn khá sát đường u0.
Ghép tầng các bộ lọc ta thu được kết quả chính xác hơn, tất nhiên nó sẽ đáp ứng trễ hơn 1 tầng
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
|
#include <SimpleKalmanFilter.h>
SimpleKalmanFilter bo_loc(2, 2, 0.001);
float u0 = 100.0; // giá trị thực (không đổi)
float e; // nhiễu
float u; // giá trị đo được (có thêm nhiễu)
float u_kalman; // giá được lọc nhiễu
void setup()
{
Serial.begin(9600);
}
void loop()
{
randomSeed(millis());
e = (float)random(–100, 100);
u = u0 + e;
Serial.print(u);
Serial.print(“,”);
u_kalman = bo_loc.updateEstimate(u); // tầng 1
u_kalman = bo_loc.updateEstimate(u_kalman); // tầng 2
u_kalman = bo_loc.updateEstimate(u_kalman); // tầng 3
u_kalman = bo_loc.updateEstimate(u_kalman); // tầng 4
Serial.print(u_kalman);
Serial.println();
}
|
Kết quả cho 4 tầng lọc
Thuật toán Kalman trong C
Đây là thuật toán đơn giản mình tìm được, hãy vào địa chỉ bên dưới để hiểu được ý nghĩa của các tham số K (Kalman Gian) ,P,Q,R…
https://malcolmmielle.wordpress.com/2015/04/29/kalman-filter/
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
|
/*
* Need to tweak value of sensor and process noise
* arguments :
* process noise covariance
* measurement noise covariance
* estimation error covariance */
class Kalman_Filter_Distance {
protected:
double _q; //process noise covariance
double _q_init;
double _r; //measurement noise covariance
double _r_init;
double _x; //value
double _p; //estimation error covariance
double _p_init;
double _k; //kalman gain
public:
/*
* Need to tweak value of sensor and process noise
* arguments :
* process noise covariance
* measurement noise covariance
* estimation error covariance */
Kalman_Filter_Distance(double q, double r, double p)
: _q(q)
, _q_init(q)
, _r(r)
, _r_init(0)
, _x(0)
, _p(p)
, _p_init(p)
, _k(_p / (_p + _r)){};
virtual void init(double x) { _x = x; }
virtual void setProcessNoiseCovariance(double i)
{
_q = i;
_q_init = i;
}
virtual void setMeasurementNoiseCovariance(double i)
{
_r = i;
_r_init = i;
}
virtual void setEstimatiomErrorCovariance(double i)
{
_p = i;
_p_init = i;
}
virtual double kalmanUpdate(double measurement);
virtual void reset()
{
_q = _q_init;
_r = _r_init;
_p = _p_init;
};
};
double Kalman_Filter_Distance::kalmanUpdate(double measurement)
{
//prediction update
//omit _x = _x
_p = _p + _q;
//measurement update
_k = _p / (_p + _r);
_x = _x + _k * (measurement – _x);
_p = (1 – _k) * _p;
return _x;
}
|
Thư viện lọc Kalman đơn giản
Của tác giả denyssene : SimpleKalmanFilter
Link dự phòng : https://drive.google.com/file/d/0BzM…
Test thư viện với la bàn số
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
|
#include <Wire.h>
#include <HMC5883L.h>
#include <SimpleKalmanFilter.h>
SimpleKalmanFilter pressureKalmanFilter(1, 1, 0.001);
HMC5883L compass;
float x_kalman;
void checkSettings()
{
}
void setup()
{
Serial.begin(9600);
// Initialize HMC5883L
Serial.println(“Initialize HMC5883L”);
while (!compass.begin()) {
Serial.println(“Could not find a valid HMC5883L sensor, check wiring!”);
delay(500);
}
compass.setRange(HMC5883L_RANGE_1_3GA);
compass.setMeasurementMode(HMC5883L_CONTINOUS);
compass.setDataRate(HMC5883L_DATARATE_15HZ);
compass.setSamples(HMC5883L_SAMPLES_1);
// Check settings
checkSettings();
}
void loop()
{
Vector raw = compass.readRaw();
Vector norm = compass.readNormalize();
x_kalman = pressureKalmanFilter.updateEstimate(raw.XAxis);
Serial.print(raw.XAxis);
Serial.print(“,”);
Serial.print(x_kalman, 3);
Serial.println();
delay(100);
}
|
Test thực tế với cảm biến áp suất
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
|
#include <SimpleKalmanFilter.h>
#include <SFE_BMP180.h>
/*
This sample code demonstrates how the SimpleKalmanFilter object can be used with a
pressure sensor to smooth a set of altitude measurements.
This example needs a BMP180 barometric sensor as a data source.
https://www.sparkfun.com/products/11824
SimpleKalmanFilter(e_mea, e_est, q);
e_mea: Measurement Uncertainty
e_est: Estimation Uncertainty
q: Process Noise
*/
SimpleKalmanFilter pressureKalmanFilter(1, 1, 0.01);
SFE_BMP180 pressure;
// Serial output refresh time
const long SERIAL_REFRESH_TIME = 100;
long refresh_time;
float baseline; // baseline pressure
double getPressure()
{
char status;
double T, P;
status = pressure.startTemperature();
if (status != 0) {
delay(status);
status = pressure.getTemperature(T);
if (status != 0) {
status = pressure.startPressure(3);
if (status != 0) {
delay(status);
status = pressure.getPressure(P, T);
if (status != 0) {
return (P);
}
}
}
}
}
void setup()
{
Serial.begin(115200);
// BMP180 Pressure sensor start
if (!pressure.begin()) {
Serial.println(“BMP180 Pressure Sensor Error!”);
while (1)
; // Pause forever.
}
baseline = getPressure();
}
void loop()
{
float p = getPressure();
float altitude = pressure.altitude(p, baseline);
float estimated_altitude = pressureKalmanFilter.updateEstimate(altitude);
if (millis() > refresh_time) {
Serial.print(altitude, 6);
Serial.print(“,”);
Serial.print(estimated_altitude, 6);
Serial.println();
refresh_time = millis() + SERIAL_REFRESH_TIME;
}
}
|
Test thư viện với cảm biến gia tốc
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
|
#include <SimpleKalmanFilter.h>
#include <Wire.h>
#include “I2C.h”
#define RESTRICT_PITCH
SimpleKalmanFilter simpleKalmanFilter(1, 1, 0.001);
uint8_t i2cData[14]; // Buffer for I2C data
int16_t accX;
float accX_kalman;
void setup()
{
Serial.begin(9600);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) – 16) / 2; // Set I2C frequency to 400kHz
#endif
i2cData[0] = 7; // Set the sample rate to 1000Hz – 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x03; // Set Accelerometer Full Scale Range to ±16g
while (i2cWrite(0x19, i2cData, 4, false))
; // Write to all four registers at once
while (i2cWrite2(0x6B, 0x01, true))
; // PLL with X axis gyroscope reference and disable sleep mode
}
void loop()
{
while (i2cRead(0x3B, i2cData, 14)) {
;
}
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accX_kalman = simpleKalmanFilter.updateEstimate((float)accX);
Serial.print(accX);
Serial.print(“,”);
Serial.print(accX_kalman, 3);
Serial.println();
}
|
Nguồn Arduino: Download
Một vài thư viện hay về bộ lọc Kalman
Của tác giả bachagas :https://github.com/bachagas/Kalman…
Link dự phòng: https://drive.google.com/file/d/0BzM…
Thư viện lọc nhiễu do cảm biến góc PMU6050 :
Của tác giả Lauszus : https://github.com/TKJElectronics/Ka…
Link dự phòng: https://drive.google.com/file/d/0BzM…
Thư viện Kalman đơn giản :
Của tác giả denyssene : https://github.com/denyssene/SimpleK…
Link dự phòng: https://drive.google.com/file/d/0BzM…
Tài liệu sưu tầm Tại đây
Nghiên cứu lọc thích nghi – kalman cho cảm biến gia tốc trên xe lăn điện: Tại đây
Ứng dụng bộ lọc kalman trong việc xử lý tín hiệu thu được từ cảm biến đo nồng độ bụi SHARP GP2Y1010AU0F: Tại đây
Ví dụ với Arduino
Lọc nhiễu cho cảm biến sử dụng bộ lọc Kalman với cảm biến độ ẩm đất tại chân A0 của Arduino Uno: Tại đây
Vài thông tin khác về chống nhiễu cho cảm biến
Các phương pháp chống nhiễu tín hiệu bằng phần cứng là:
– Dùng tụ để lọc bỏ các gai nhiễu tần số cao.
– Dùng khuyếch đại vi sai để triệt nhiễu đồng pha.
– Chuyển tín hiệu từ áp sang dòng để truyền tải vì tín hiệu dòng không tổn hao do dây dẫn. Mạch chuyển tín hiệu có thể dùng Op-amp hay IC chuyên dụng.
, một kỹ sư điện, nhà toán học, nhà phát minh người Mỹ gốc Hungary.){kind=link}