Bộ lọc Kalman là gì: Kalman một chiều cho cảm biến, chọn Q và R, quan hệ với lọc EMA

Bộ lọc Kalman là gì, giải thích bằng ví dụ một chiều: dự đoán và cập nhật, ý nghĩa Q, R, P, K; mô phỏng ba giá trị Q; K ổn định và quan hệ với EMA; cách chọn Q, R; code ESP32 tự đo nhiễu R.

Bộ lọc Kalman nổi tiếng là khó: tài liệu thường bắt đầu bằng ma trận, xác suất, hiệp phương sai. Nhưng với một số đo duy nhất (nhiệt độ, khoảng cách, điện áp, mực nước), bộ lọc Kalman một chiều chỉ gồm vài dòng code và có thể hiểu bằng trực giác. Bài này giải thích bộ lọc Kalman là gì qua ví dụ một chiều: hai bước dự đoán và cập nhật, ý nghĩa của Q, R, P và hệ số K, cách chọn Q, R cho cảm biến thật. Bài cũng chỉ ra một điều ít tài liệu nói: ở trạng thái ổn định, Kalman một chiều chính là lọc EMA. Có script Python mô phỏng và chương trình ESP32 tự đo nhiễu R.

1. Ý tưởng: kết hợp dự đoán với số đo

Bộ lọc giữ hai con số:

• x: ước lượng hiện tại của giá trị thật;
• P: độ không chắc chắn của ước lượng đó (phương sai).

Mỗi khi có số đo mới, bộ lọc làm hai bước:

1. Dự đoán: chưa biết gì mới thì đoán giá trị vẫn như cũ, nhưng kém chắc chắn hơn vì giá trị thật có thể đã đổi: P = P + Q.
2. Cập nhật: so số đo z với dự đoán, sửa ước lượng một phần K của độ chênh: x = x + K (z − x). Hệ số K = P / (P + R) cân giữa độ không chắc chắn của dự đoán (P) và của số đo (R). Sau đó ước lượng chắc chắn hơn: P = (1 − K) P.

• R: phương sai nhiễu của cảm biến. Số đo nhảy ±20 quanh giá trị thật (độ lệch chuẩn 20) thì R = 400.
• Q: giá trị thật có thể thay đổi bao nhiêu giữa hai lần đo (cũng là phương sai). Nhiệt độ phòng đo mỗi giây thay đổi rất ít, Q nhỏ; mực nước bể đang bơm thay đổi nhanh, Q lớn hơn.

Số đo nhiễu nhiều (R lớn) so với độ không chắc chắn của dự đoán (P) thì K nhỏ: bộ lọc ít tin số đo, ước lượng mượt. Ngược lại K gần 1: bộ lọc bám sát số đo.

2. Ví dụ: cùng số đo, ba giá trị Q

Tín hiệu thử là một giá trị đứng yên ở 1000, nhảy lên 1200 ở mẫu 150, rồi tăng dần từ mẫu 280; số đo có nhiễu ngẫu nhiên độ lệch chuẩn 20. Lọc bằng R = 400 và ba giá trị Q:

• Q = 0,05: rất mượt khi giá trị đứng yên, nhưng mất hàng trăm mẫu mới theo kịp bước nhảy, và luôn trễ khi giá trị tăng dần.
• Q = 0,5: cân bằng giữa mượt và nhanh.
• Q = 20: bám rất nhanh nhưng còn nhiễu nhiều.

Không có Q "đúng" cho mọi việc: chọn theo mức thay đổi thật của đại lượng đo và độ trễ chấp nhận được.

# PYWORLD.VN - Bộ lọc Kalman một chiều cho số đo cảm biến: dự đoán, cập nhật, hệ số Kalman K, phương sai P
# PYWORLD.VN - Tính K ổn định (K∞) từ Q và R: khi đó Kalman 1D trùng với lọc EMA có alpha = K∞
# PYWORLD.VN - Chạy: python kalman_1d.py --q 0.5 --r 400   (Q: nhiễu quá trình, R: phương sai nhiễu đo)
"""Kalman 1D (mô hình giá trị đi ngẫu nhiên): mô phỏng, K ổn định, so với EMA."""
from __future__ import annotations

import argparse
import math
import random


class Kalman1D:
    def __init__(self, q: float, r: float, x0: float, p0: float):
        self.q, self.r, self.x, self.p = q, r, x0, p0
        self.k = 0.0

    def buoc(self, z: float) -> float:
        self.p += self.q                      # dự đoán: giá trị giữ nguyên, độ bất định tăng thêm Q
        self.k = self.p / (self.p + self.r)   # hệ số Kalman: tin số đo bao nhiêu
        self.x += self.k * (z - self.x)       # cập nhật theo sai lệch giữa số đo và dự đoán
        self.p *= 1 - self.k                  # độ bất định sau cập nhật giảm đi
        return self.x


def k_on_dinh(q: float, r: float) -> float:
    """P⁻ thỏa P⁻² − Q·P⁻ − Q·R = 0; K∞ = Q / P⁻."""
    p_truoc = (q + math.sqrt(q * q + 4 * q * r)) / 2
    return q / p_truoc


def tin_hieu(n=400, seed=5, sigma=20.0):
    """Giá trị thật: 1000, nhảy lên 1200 ở mẫu 150, tăng dần 0,5 mỗi mẫu từ mẫu 280; số đo có nhiễu chuẩn sigma."""
    rnd = random.Random(seed)
    that, do = [], []
    for i in range(n):
        v = 1000.0 if i < 150 else 1200.0
        if i >= 280:
            v += 0.5 * (i - 280)
        that.append(v)
        do.append(v + rnd.gauss(0, sigma))
    return that, do


def chay(q, r, do):
    kf = Kalman1D(q, r, do[0], r)
    ra, ks = [], []
    for z in do:
        ra.append(kf.buoc(z))
        ks.append(kf.k)
    return ra, ks


def rms(a, b, tu=0, den=None):
    den = den or len(a)
    return math.sqrt(sum((a[i] - b[i]) ** 2 for i in range(tu, den)) / (den - tu))


def main() -> None:
    ap = argparse.ArgumentParser(description=__doc__)
    ap.add_argument("--q", type=float, default=0.5, help="nhiễu quá trình Q (giá trị thật đổi bao nhiêu mỗi mẫu, bình phương)")
    ap.add_argument("--r", type=float, default=400.0, help="phương sai nhiễu đo R (độ lệch chuẩn 20 thì R = 400)")
    a = ap.parse_args()
    that, do = tin_hieu()
    r_uoc = sum((z - sum(do[:150]) / 150) ** 2 for z in do[:150]) / 149
    print(f"Uoc luong R tu 150 mau dau (gia tri dung yen): {r_uoc:.0f} (that: 400)")
    k_inf = k_on_dinh(a.q, a.r)
    print(f"Q = {a.q:g}, R = {a.r:g} -> K on dinh = {k_inf:.4f} (tuong duong EMA alpha {k_inf:.4f})")
    for q in (0.05, a.q, 20.0):
        ra, ks = chay(q, a.r, do)
        print(f"  Q {q:5g}: K sau 10 mau {ks[9]:.3f}, K cuoi {ks[-1]:.4f} | sai so RMS doan dung yen {rms(ra, that, 20, 150):5.1f},"
              f" doan tang dan {rms(ra, that, 300):5.1f} (so do tho {rms(do, that):.1f})")
    ema, y = [], do[0]
    for z in do:
        y += k_inf * (z - y)
        ema.append(y)
    ra, _ = chay(a.q, a.r, do)
    print(f"Chenh lech Kalman (Q {a.q:g}) va EMA alpha {k_inf:.4f} sau mau 100: lon nhat {max(abs(ra[i] - ema[i]) for i in range(100, len(do))):.4f}")


if __name__ == "__main__":
    main()

Script đo sai số RMS so với giá trị thật trên hai đoạn: đoạn đứng yên và đoạn tăng dần. Với số đo thô sai 20,8:

• Q = 0,5 cho sai 2,4 ở đoạn đứng yên và 12,2 ở đoạn tăng dần;
• Q = 0,05 mượt hơn ở đoạn đứng yên (1,7) nhưng sai tới 44,8 ở đoạn tăng dần;
• Q = 20 làm ngược lại.

3. Kalman một chiều và EMA

Với Q, R cố định, hệ số K giảm dần trong vài chục mẫu đầu rồi đứng yên ở một giá trị K∞. Khi K không đổi, công thức cập nhật x = x + K (z − x) chính là lọc EMA với alpha = K∞. K∞ tính trực tiếp được từ Q và R:

P⁻ = (Q + √(Q² + 4·Q·R)) / 2       K∞ = Q / P⁻

Script kiểm chứng: với Q = 0,5, R = 400, K∞ = 0,0347; sau mẫu 100, Kalman và EMA alpha 0,0347 chênh nhau chưa tới 0,6 đơn vị.

Kalman một chiều vẫn có ích hơn EMA ở hai điểm:

• Khởi động nhanh: lúc đầu P lớn nên K lớn, bộ lọc theo kịp giá trị thật ngay, sau đó mới mượt dần.
• Chọn tham số theo ý nghĩa vật lý: R đo được từ cảm biến, Q suy từ mức thay đổi của đại lượng, thay vì thử alpha.

Ngoài ra, khi muốn cho phép R thay đổi theo lúc (cảm biến lúc tốt lúc kém), hoặc bỏ qua một số đo hỏng (đặt R rất lớn cho mẫu đó), Kalman làm được còn EMA thì không. Phần EMA và cách viết bằng số nguyên có ở bài lọc EMA cùng cụm chủ đề.

4. Chương trình ESP32

// PYWORLD.VN - Bộ lọc Kalman một chiều cho số đọc ADC trên ESP32: dự đoán, cập nhật, in giá trị thô, đã lọc, hệ số K cho Serial Plotter
// PYWORLD.VN - Lệnh "r" đo phương sai nhiễu R từ 200 mẫu khi cảm biến đứng yên; lệnh "q 0.5" đổi nhiễu quá trình Q
// PYWORLD.VN - Cảm biến analog hoặc biến trở ở GPIO34 (ADC1), lấy mẫu 100 lần/giây

const int CHAN_ADC = 34;
const uint32_t CHU_KY_US = 10000;            // 100 Hz

struct Kalman1D {
  float q, r, x, p, k;
  float buoc(float z) {
    p += q;                                 // dự đoán: giá trị giữ nguyên, độ bất định tăng thêm Q
    k = p / (p + r);                        // hệ số Kalman
    x += k * (z - x);                       // cập nhật theo sai lệch số đo - dự đoán
    p *= 1 - k;
    return x;
  }
};

Kalman1D kf = {0.5f, 400.0f, 0, 400.0f, 0};
bool daKhoiTao = false;
int soMauDoR = 0;
double tongDoR = 0, tongBinhPhuongDoR = 0;

float kOnDinh(float q, float r) {
  float pTruoc = (q + sqrtf(q * q + 4 * q * r)) / 2;
  return q / pTruoc;
}

void xuLyLenh() {
  String s = Serial.readStringUntil('\n');
  s.trim();
  if (s == "r") {
    soMauDoR = 200;
    tongDoR = tongBinhPhuongDoR = 0;
    Serial.println("# Do R: giu cam bien dung yen 2 giay");
  } else if (s.startsWith("q ")) {
    kf.q = s.substring(2).toFloat();
  } else if (s.startsWith("rr ")) {
    kf.r = s.substring(3).toFloat();
  }
  Serial.printf("# Q %.3f, R %.1f -> K on dinh %.4f\n", kf.q, kf.r, kOnDinh(kf.q, kf.r));
}

void setup() {
  Serial.begin(115200);
  analogSetPinAttenuation(CHAN_ADC, ADC_11db);
  Serial.println("# Lenh: r = do R, q 0.5 = doi Q, rr 400 = dat R bang tay");
}

void loop() {
  static uint32_t tTiep = micros();
  static int dem = 0;
  if (Serial.available()) xuLyLenh();
  if ((int32_t)(micros() - tTiep) < 0) return;
  tTiep += CHU_KY_US;

  float z = analogRead(CHAN_ADC);
  if (!daKhoiTao) {                          // khởi tạo bằng mẫu đầu, P = R: chưa tin hơn một lần đo
    kf.x = z;
    kf.p = kf.r;
    daKhoiTao = true;
  }
  if (soMauDoR > 0) {                        // cộng dồn để tính phương sai số đo khi đứng yên
    tongDoR += z;
    tongBinhPhuongDoR += (double)z * z;
    if (--soMauDoR == 0) {
      double tb = tongDoR / 200;
      kf.r = (float)((tongBinhPhuongDoR - 200 * tb * tb) / 199);
      Serial.printf("# R do duoc = %.1f (do lech chuan %.1f)\n", kf.r, sqrtf(kf.r));
    }
  }
  float x = kf.buoc(z);
  if (++dem >= 2) {                          // 50 dòng/giây
    dem = 0;
    Serial.printf("tho:%.0f,kalman:%.1f,K_x1000:%.1f\n", z, x, kf.k * 1000);
  }
}

Nối biến trở hoặc cảm biến analog vào GPIO34, mở Serial Plotter ở 115200 baud để thấy số đo thô, giá trị đã lọc và K (nhân 1000 cho dễ nhìn). Các lệnh trong Serial Monitor:

• r: giữ cảm biến đứng yên khoảng 2 giây, chương trình lấy 200 mẫu và tính phương sai làm R.
• q 0.5: đổi Q để thử độ mượt và độ trễ.
• rr 400: đặt R bằng tay.

Mỗi lần đổi, chương trình in ra K∞ để biết bộ lọc tương đương EMA nào.

5. Chọn Q và R cho cảm biến thật

1. Đo R: giữ đại lượng đo không đổi (cảm biến nhiệt trong cốc nước ổn định, cảm biến khoảng cách nhìn vào tường), ghi 100–200 mẫu, tính phương sai. Phương sai từ vài trăm mẫu vẫn có sai số (trong ví dụ, ước lượng 510 so với 400 thật), nhưng lệch vài chục phần trăm không làm bộ lọc tệ đi nhiều.
2. Ước lượng Q từ mức thay đổi thật giữa hai mẫu: nếu đại lượng có thể đổi tới khoảng d đơn vị mỗi mẫu, bắt đầu với Q ≈ d².
3. Thử và chỉnh Q trên Serial Plotter: tăng Q nếu bộ lọc trễ quá, giảm Q nếu còn nhiễu.

6. Từ một chiều tới Kalman mở rộng

• Nhiều trạng thái: ước lượng cùng lúc vị trí và vận tốc, x trở thành vectơ, P, Q, R thành ma trận. Bộ lọc khi đó dự đoán được cả xu hướng (giá trị đang tăng thì dự đoán tăng tiếp), không bị trễ khi giá trị tăng đều như Kalman một chiều.
• Kết hợp nhiều cảm biến: ví dụ gia tốc kế và con quay hồi chuyển để tính góc nghiêng; bộ lọc bù trong bài ESP32 đọc MPU-6050 tính góc nghiêng bằng bộ lọc bù là phiên bản đơn giản của ý tưởng này.
• Kalman mở rộng (EKF): dùng khi quan hệ giữa trạng thái và số đo không tuyến tính (định vị GPS, robot di chuyển), bằng cách tuyến tính hóa quanh điểm làm việc mỗi bước.

7. Lỗi hay gặp

8. Xem thêm

• ESP32 đọc MPU-6050 tính góc nghiêng bằng bộ lọc bù
• Đọc ADC trên ESP32 và đo điện áp pin 18650
• ESP32 DevKit V1 30 chân, cổng Type-C

Bài cùng cụm chủ đề: lọc EMA thông thấp một cực; lọc trung bình trượt và trung vị.