Lọc nhiễu tín hiệu cảm biến - từ trung bình trượt đến Kalman một chiều

Cảm biến không nói dối, nhưng nói... run

Đặt HC-SR04 trước bức tường đứng yên và in giá trị: 57.2, 57.4, 56.9, 61.8 (?), 57.1... Tường không nhúc nhích — con số nhảy múa. Đó là nhiễu: bạn đồng hành bất khả kháng của mọi phép đo, đến từ nguồn gợn, motor phóng điện, rung cơ khí và bản chất linh kiện giá rẻ. Code quyết định dựa trên giá trị thô là robot giật cục theo từng cú nhảy của nhiễu.

Lọc nhiễu là nghệ thuật đánh đổi: mượt hơn luôn phải trả bằng trễ hơn. Bài này xếp 5 bộ lọc từ đơn giản đến Kalman theo đúng trục đánh đổi đó — kèm quy tắc chọn theo loại nhiễu, vì bộ lọc sai loại còn tệ hơn không lọc.

Bước 0 - diệt nhiễu tại gốc trước khi lọc

Lọc phần mềm là tuyến phòng thủ THỨ HAI. Tuyến đầu là phần cứng, rẻ và hiệu quả hơn mọi thuật toán: tụ 100nF sát chân nguồn cảm biến, dây tín hiệu ngắn và tránh xa dây motor, cảm biến bắt chặt hết rung, chung GND một điểm. Nhiễu giảm 10 lần từ gốc thì bộ lọc nhẹ nhàng 10 lần. (Chi tiết ở các bài cảm biến trong mục Kiến thức.)

Bộ lọc 1 - Trung vị (median): sát thủ của giá trị ma

HC-SR04 thỉnh thoảng trả 400 (mất echo) giữa chuỗi 57 — đó là outlier (giá trị ma), và trung bình cộng bị nó kéo lệch thảm hại. Trung vị thì miễn nhiễm: đo 3-5 lần, lấy giá trị ĐỨNG GIỮA sau khi xếp thứ tự.

long medianOf3(long a, long b, long c) {
  if (a > b) { long t = a; a = b; b = t; }
  if (b > c) { long t = b; b = c; c = t; }
  return (a > b) ? a : b;
}
long distance = medianOf3(ping(), ping(), ping());

Chọn khi: nhiễu dạng xung lạc lõng (siêu âm mất echo, encoder nhiễu xung, chạm chập chờn). Giá phải trả: đo nhiều lần — chậm đi tương ứng.

Bộ lọc 2 - Trung bình trượt (moving average): con ngựa thồ

Cộng N mẫu gần nhất chia N — nhiễu ngẫu nhiên triệt tiêu nhau, tín hiệu thật ở lại:

const int N = 8;
int buf[N]; int idx = 0; long sum = 0;

int smooth(int raw) {
  sum -= buf[idx];
  buf[idx] = raw;
  sum += raw;
  idx = (idx + 1) % N;
  return sum / N;
}

N là núm vặn đánh đổi: N=4 mượt nhẹ trễ ít, N=16 mượt sâu trễ rõ (trễ nhóm ≈ N/2 mẫu). Chọn khi: nhiễu lăn tăn đều quanh giá trị thật (analog quang trở, dòng điện, pin). Tránh khi: tín hiệu đổi nhanh cần bám sát — trễ N/2 mẫu giết vòng điều khiển nhanh.

Bộ lọc 3 - EMA: trung bình trượt phiên bản một dòng

Exponential Moving Average — bản "lười" của trung bình trượt, không cần mảng:

float ema = 0;
// alpha 0.1 = mượt sâu/trễ nhiều; 0.5 = bám nhanh/mượt nông
ema += 0.2 * (raw - ema);

Cùng họ đánh đổi với moving average nhưng: một biến, một phép nhân, chỉnh mượt-trễ bằng MỘT số thực (alpha) thay vì đổi cỡ mảng. Vì gọn nên nó là bộ lọc mặc định nên thử đầu tiên cho mọi tín hiệu analog. Mẹo chọn alpha nhanh: bắt đầu 0.2; run quá → giảm nửa; đuối nhịp → tăng gấp rưỡi.

Bộ lọc 4 - Lọc bù (complementary): trộn hai cảm biến

Khi có HAI nguồn đo cùng một đại lượng với điểm mạnh yếu ngược nhau, trộn chúng thắng cả hai: gyro mượt tức thời nhưng trôi dài hạn + gia tốc kế đúng dài hạn nhưng nhiễu tức thời = góc nghiêng vừa mượt vừa không trôi:

angle = 0.98 * (angle + gyroRate * dt) + 0.02 * accAngle;

Một dòng — và là trái tim của robot cân bằng (đã dùng ở bài MPU6050 và bài Robot tự cân bằng trong mục Kiến thức). Hệ số 0.98/0.02 chính là "niềm tin" phân bổ: tin gyro 98% ở nhịp ngắn, để gia tốc kế kéo về sự thật ở nhịp dài.

Bộ lọc 5 - Kalman một chiều: lọc có não

Kalman làm điều các bộ lọc trên không làm: tự điều chỉnh độ tin theo tình hình. Nó duy trì ước lượng kèm độ bất định; mỗi mẫu mới được cân nhắc "tin bao nhiêu" dựa trên tỉ lệ nhiễu đo với nhiễu quá trình — thay vì hệ số cứng như EMA.

Bản 1D đủ dùng và đủ hiểu:

struct Kalman1D {
  float q = 0.01;   // nhiễu quá trình: tín hiệu thật đổi nhanh cỡ nào
  float r = 4.0;    // nhiễu đo: cảm biến run cỡ nào (phương sai)
  float x = 0;      // ước lượng hiện tại
  float p = 1;      // độ bất định hiện tại

  float update(float z) {
    p += q;                    // dự đoán: bất định tăng theo thời gian
    float k = p / (p + r);     // hệ số Kalman: cân "tin đo hay tin mình"
    x += k * (z - x);          // sửa ước lượng theo mẫu mới
    p *= (1 - k);              // đo xong thì bớt bất định
    return x;
  }
};

Nhìn kỹ dòng x += k * (z - x): chính là EMA với alpha tự động — Kalman 1D là EMA có não. Chỉnh hai núm: r đo từ thực tế (ghi 100 mẫu khi tín hiệu đứng yên, tính phương sai), q chỉnh cảm tính (tín hiệu thật đổi chậm → q nhỏ → mượt sâu). Đáng dùng khi: tín hiệu lúc đứng yên lúc đổi nhanh (khoảng cách robot đang chạy-dừng) — EMA hệ số cứng phải hy sinh một trong hai, Kalman ăn cả hai.

Bảng chọn bộ lọc theo triệu chứng

Nguyên tắc xếp tầng: trung vị luôn đứng trước (diệt ma trước khi làm mượt — trung bình hóa giá trị ma là ma lan vào N mẫu).

Đo hiệu quả lọc bằng mắt - đừng chỉnh mò

Serial Plotter (bài Debug bằng Serial Monitor ở mục Kiến thức) in song song hai kênh:

Serial.printf("raw:%d,filtered:%.1f\n", raw, filtered);

Nhìn đồ thị đánh giá được cả hai mặt của đánh đổi: đường lọc phẳng chưa (độ mượt) và khi bạn vẫy tay trước cảm biến, đường lọc đuổi kịp đường thô sau bao lâu (độ trễ). Chỉnh alpha/N/q-r đến điểm cân bằng mà VÒNG ĐIỀU KHIỂN của bạn chịu được — robot cân bằng chịu trễ kém hơn trạm thời tiết cả trăm lần, không có bộ số đúng chung.

Câu hỏi thường gặp

Lọc ở đâu: trong hàm đọc cảm biến hay trong vòng điều khiển?

Trong hàm đọc — trả về giá trị đã lọc, vòng điều khiển không cần biết. Đóng gói kiểu này (một struct/hàm mỗi cảm biến, giữ trạng thái lọc riêng) giúp thay bộ lọc không đụng logic robot, đúng tinh thần tách hàm của bài Hàm và thư viện Arduino.

Có nên lọc tín hiệu digital (nút nhấn, dò line D0) không?

Có, nhưng bằng họ khác: debounce theo thời gian (mẫu trong bài Đa nhiệm với millis) cho nút nhấn; xác nhận N lần liên tiếp cho dò line digital. Các bộ lọc số học trong bài này dành cho tín hiệu liên tục.

Kalman "xịn" nhiều chiều khác gì bản 1D này?

Bản đầy đủ ước lượng nhiều biến ràng buộc nhau (vị trí + vận tốc + hướng) bằng ma trận — nền của định vị robot tự hành thật. Bản 1D giữ nguyên linh hồn (cân bằng niềm tin dự đoán-phép đo) bỏ đại số ma trận — đúng điểm dừng cho phổ thông, và là bệ phóng nếu học tiếp lên đại học.

PID đã có phần D nhạy nhiễu — lọc trước hay chỉnh D nhỏ?

Cả hai là một bài toán: D khuếch đại nhiễu tần số cao, nên tín hiệu vào PID nhanh (dò line, cân bằng) gần như luôn cần lọc nhẹ (EMA alpha lớn 0.3-0.5 — mượt nông giữ nhịp) TRƯỚC khi vào PID, thay vì bóp D đến mất tác dụng. Trễ lọc quá sâu lại phá pha của D — thêm một lý do để chỉnh bằng đồ thị chứ không mò.