Bộ lọc Kalman (Kalman Filter)

Yêu cầu dự đoán

Chúng ta bắt đầu bằng cách mô hình hóa bài toán để hiểu vì sao cần một thuật toán cho ước lượng trạng thái và dự đoán.

Để minh họa, hãy xét ví dụ ra-đa bám theo mục tiêu:

Giả sử chúng ta có một ra-đa bám theo máy bay. Trong bối cảnh này, máy bay là hệ thống, và đại lượng cần ước lượng là vị trí của nó, tương ứng với trạng thái hệ thống.

Ra-đa lấy mẫu mục tiêu bằng cách điều khiển một bó tia hẹp hướng về nó và cung cấp các phép đo vị trí của máy bay. Dựa trên các phép đo này, ta có thể ước lượng trạng thái hệ thống (vị trí của máy bay).

Để bám theo máy bay, ra-đa phải quay lại mục tiêu theo chu kỳ bằng cách hướng bó tia theo đúng hướng. Điều đó có nghĩa ra-đa cần dự đoán vị trí tương lai của máy bay cho lần quét tiếp theo. Nếu không dự đoán được, bó tia có thể lệch hướng, dẫn tới mất bám. Để dự đoán, ta cần một số hiểu biết về cách máy bay di chuyển. Nói cách khác, ta cần một mô hình mô tả hành vi hệ thống theo thời gian, gọi là mô hình động (dynamic model).

Để đơn giản hóa ví dụ, hãy xét thế giới một chiều (một đường thẳng) trong đó máy bay di chuyển theo đường thẳng tiến về phía ra-đa hoặc rời xa nó.

Trạng thái hệ thống được xác định là khoảng cách của máy bay tới ra-đa, ký hiệu \( r \). Ra-đa phát một xung về phía máy bay, xung phản xạ từ mục tiêu và quay về ra-đa. Bằng cách đo thời gian giữa truyền và nhận xung, đồng thời biết xung là sóng điện từ truyền với tốc độ ánh sáng, ra-đa dễ dàng tính khoảng cách \( r \) của máy bay. Ngoài khoảng cách, ra-đa cũng đo được vận tốc \( v \) của máy bay, tương tự cách súng ra-đa của cảnh sát đo tốc độ xe bằng hiệu ứng Doppler.

Giả sử tại thời điểm \( t_{0} \), ra-đa đo khoảng cách và vận tốc của máy bay với độ chính xác và độ chính xác cao. Khoảng cách đo được là 10.000 mét và vận tốc là 200 mét mỗi giây. Điều này cho ta trạng thái hệ thống:

\[ r_{t_{0}} = 10,000m \]

Bước tiếp theo là dự đoán trạng thái hệ thống tại thời điểm \( t_{1}=t_{0}+\Delta t \), trong đó \( \Delta t \) là chu kỳ quay lại mục tiêu. Giả sử máy bay duy trì vận tốc không đổi, ta có thể dùng mô hình động vận tốc không đổi (constant-velocity dynamic model) để dự đoán vị trí tương lai của nó.

Quãng đường đi được trong khoảng thời gian \( \Delta t \) được cho bởi:

\[ \Delta r = v \cdot \Delta t \]

Giả sử khoảng lấy mẫu là 5 giây, vị trí dự đoán tại thời điểm \( t_{1} \) là:

\[ r_{t_{1}} = r_{t_{0}} + \Delta r = 10,000 + 200 \cdot 5 = 11,000m \]

Đây là một thuật toán sơ cấp dựa trên các nguyên lý đơn giản. Trạng thái hệ thống hiện tại được suy ra từ phép đo, và mô hình động được dùng để dự đoán trạng thái tương lai.

Trong thực tế, mọi thứ phức tạp hơn. Trước hết, các phép đo của ra-đa không hoàn toàn chính xác; chúng chịu ảnh hưởng của nhiễu và chứa mức độ ngẫu nhiên nhất định. Nếu mười ra-đa khác nhau cùng đo khoảng cách của máy bay tại một thời điểm, chúng sẽ cho ra mười kết quả hơi khác nhau. Các kết quả có thể gần nhau nhưng không trùng khớp. Sự khác biệt trong phép đo là do nhiễu đo lường (measurement noise).

Điều này dẫn đến câu hỏi mới: Mức độ tin cậy của ước lượng là bao nhiêu? Chúng ta cần một thuật toán không chỉ đưa ra ước lượng mà còn cho biết độ tin cậy của ước lượng đó.

Một vấn đề khác là độ chính xác của mô hình động. Dù ta giả định máy bay di chuyển với vận tốc không đổi, các yếu tố bên ngoài như gió có thể gây lệch so với giả định. Những ảnh hưởng khó dự đoán này được gọi là nhiễu quá trình (process noise).

Tương tự như việc đánh giá độ tin cậy của ước lượng dựa trên đo lường, chúng ta cũng muốn hiểu mức độ tin cậy của dự đoán.

Bộ lọc Kalman là thuật toán ước lượng trạng thái cung cấp cả ước lượng trạng thái hiện tại và dự đoán trạng thái tương lai, kèm theo thước đo về độ không đảm bảo của chúng. Hơn nữa, đây là một thuật toán tối ưu giúp giảm thiểu độ không đảm bảo của ước lượng trạng thái. Đó là lý do Bộ lọc Kalman được sử dụng rộng rãi và đáng tin cậy.

Ví dụ về Bộ lọc Kalman

Hãy bắt đầu với một ví dụ đơn giản: ra-đa một chiều đo khoảng cách và vận tốc bằng cách phát một xung về phía máy bay và thu xung phản xạ. Độ trễ thời gian giữa phát xung và nhận xung cung cấp thông tin về khoảng cách \(r\) của máy bay, còn độ dịch tần của xung phản xạ cung cấp thông tin về vận tốc \(v\) (hiệu ứng Doppler).

Trong ví dụ này, trạng thái hệ thống được mô tả bởi cả khoảng cách \(r\) và vận tốc \(v\) của máy bay. Ta định nghĩa trạng thái hệ thống bằng vectơ \(\boldsymbol{x}\), bao gồm cả hai đại lượng:

\[ \boldsymbol{x}=\left[\begin{matrix}r\\v\\\end{matrix}\right] \]

Ta ký hiệu vectơ bằng chữ thường in đậm và ma trận bằng chữ hoa in đậm.

Do trạng thái hệ thống gồm hơn một biến, chúng ta sử dụng các công cụ đại số tuyến tính, như vectơ và ma trận, để mô tả toán học của Bộ lọc Kalman. Nếu bạn chưa quen với đại số tuyến tính, vui lòng xem lại phần Bộ lọc Kalman một chiều trong hướng dẫn trực tuyến hoặc trong sách. Phần này trình bày các phương trình của Bộ lọc Kalman và diễn giải của chúng bằng toán học ở mức trung học, kèm bốn ví dụ được giải chi tiết.

Vòng lặp 0

Khởi tạo bộ lọc

Trong ví dụ này, chúng ta sẽ dùng phép đo đầu tiên để khởi tạo Bộ lọc Kalman (để biết thêm về kỹ thuật khởi tạo và ảnh hưởng của chúng đến hiệu năng Bộ lọc Kalman, tham khảo Chương 21 của sách). Tại thời điểm \(t_0\), ra-đa đo khoảng cách \(10{,}000m\) và vận tốc \(200m/s\). Các phép đo được ký hiệu bằng \(\boldsymbol{z}\).
Ta xếp các phép đo vào vectơ đo \(\boldsymbol{z}\):

\[ \boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

Chỉ số dưới \(0\) biểu thị thời điểm \(t_0\).

Phép đo không phản ánh chính xác trạng thái hệ thống. Phép đo bị suy giảm bởi nhiễu ngẫu nhiên; do đó, mỗi phép đo là một biến ngẫu nhiên.

Chúng ta có thể tin cậy phép đo này không? Mức độ chắc chắn là bao nhiêu? Mỗi phép đo đi kèm một giá trị độ không đảm bảo đo lường bình phương (đôi khi gọi là sai số đo lường). Độ không đảm bảo bình phương này chính là phương sai của phép đo. Bạn có thể đọc thêm về phương sai trong phần Nền tảng cần thiết I. Để thảo luận chi tiết hơn về độ không đảm bảo đo lường, xem phần Bộ lọc Kalman một chiều.

Trong các hệ thống ra-đa, độ không đảm bảo đo lường chủ yếu được quyết định bởi tỷ lệ cường độ tín hiệu thu được so với nhiễu. Tỷ lệ tín hiệu trên nhiễu càng cao thì phương sai đo lường càng thấp, và mức độ tin cậy đối với phép đo càng cao.

Hình dưới đây so sánh trường hợp tín hiệu thấp và tín hiệu cao trong điều kiện có nhiễu.

Giả sử độ lệch chuẩn của phép đo khoảng cách là \(4m\) và độ lệch chuẩn của phép đo vận tốc là \(0.5m/s\). Vì phương sai là bình phương của độ lệch chuẩn, độ không đảm bảo đo lường bình phương (ký hiệu \(\boldsymbol{R}\)) là:

\[ \boldsymbol{R}_0=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

\(\boldsymbol{R}\) là ma trận hiệp phương sai. Các phần tử trên đường chéo chính chứa phương sai, còn các phần tử ngoài đường chéo là hiệp phương sai giữa các phép đo.

\[ \boldsymbol{R}=\left[\begin{matrix}\sigma_r^2&\sigma_{rv}^2\\[0.5em]\sigma_{vr}^2&\sigma_v^2\\\end{matrix}\right] \]

Trong ví dụ này, ta giả định sai số của phép đo khoảng cách và vận tốc không liên quan nhau, nên các phần tử ngoài đường chéo của ma trận hiệp phương sai đo được đặt bằng 0.

Để ôn lại phương sai và độ lệch chuẩn, xem Phần Kiến thức nền I của hướng dẫn trực tuyến.
Để ôn lại ma trận hiệp phương sai, xem Phần Kiến thức nền II.

Trong bước khởi tạo, thông tin duy nhất chúng ta có là một phép đo. Trong ví dụ này, phép đo và trạng thái hệ thống được mô tả bởi cùng các đại lượng (\(r\) và \(v\)). Vì vậy, ta có thể dùng phép đo như ước lượng ban đầu của trạng thái hệ thống. Điều này chỉ có thể thực hiện trong bước khởi tạo:

\[ \boldsymbol{\hat{x}}_{0,0}=\boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

Chỉ số dưới \(0,0\) có ý nghĩa như sau:

• Chỉ số thứ nhất chỉ thời điểm của hệ thống, trong ví dụ này là \(t_0\).
• Chỉ số thứ hai chỉ thời điểm thực hiện phép ước lượng, cũng là \(t_0\).

Nói cách khác, ước lượng là cho thời điểm \(t_0\), và nó cũng được tính tại thời điểm \(t_0\).

Dự đoán

Bây giờ ta dự đoán trạng thái tiếp theo. Giả sử thời gian quay lại mục tiêu là 5 giây \((\Delta t=5s)\), do đó \(t_1=5s\).

Để ước lượng trạng thái hệ thống tương lai, ta phải mô tả cách hệ thống tiến hóa theo thời gian. Trong ví dụ này, ta giả định mô hình động vận tốc không đổi (mô hình chuyển động):

\[ v_{1} = v_{0} = v \] \[ r_{1} = r_{0} + v_{0}\Delta t \]

(Để xem các ví dụ về mô hình động tăng tốc, tham khảo Chương 9 của sách.)

Hãy mô tả mô hình động ở dạng ma trận:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{0,0} \]

Chỉ số dưới \(1,0\) có ý nghĩa như sau:

• Chỉ số thứ nhất chỉ thời gian của hệ thống, là \(t_1\).
• Chỉ số thứ hai chỉ thời điểm thực hiện phép ước lượng, là \(t_0\).

Vì vậy, \( \hat{\boldsymbol{x}}_{1,0} \) là ước lượng trạng thái hệ thống tại thời điểm \(t_1\), được tính bằng thông tin sẵn có tại thời điểm \(t_0\). Nói cách khác, đó là một dự đoán trạng thái tương lai.

Ma trận \( \boldsymbol{F} \) được gọi là ma trận chuyển trạng thái và mô tả cách trạng thái hệ thống tiến hóa theo thời gian:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\left[\begin{matrix}{\hat{r}}_{1,0}\\{\hat{v}}_{1,0}\\\end{matrix}\right]=\left[\begin{matrix}1&\Delta t\\0&1\\\end{matrix}\right]\left[\begin{matrix}{\hat{r}}_{0,0}\\{\hat{v}}_{0,0}\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}10,000\\200\\\end{matrix}\right]=\left[\begin{matrix}11,000\\200\\\end{matrix}\right] \]

Phụ lục C của sách mô tả phương pháp mô hình hóa động lực của bất kỳ hệ tuyến tính nào.

Phương trình

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} \]

là phương trình ngoại suy (dự đoán) trạng thái. Nó cho chúng ta biết cách tính trạng thái tiếp theo từ trạng thái hiện tại. Nó lấy ước lượng trạng thái hiện tại và sử dụng mô hình chuyển động của hệ thống để dự đoán trạng thái ở bước thời gian kế tiếp.

Dạng đầy đủ của phương trình ngoại suy trạng thái là:

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]

trong đó:

• \(\boldsymbol{u}_{n}\) là biến đầu vào
• \(\boldsymbol{G}\) là ma trận chuyển tiếp đầu vào

Vector đầu vào biểu diễn thông tin bổ sung cung cấp cho Bộ lọc Kalman (Kalman Filter), chẳng hạn như số đo từ cảm biến gia tốc gắn trên thiết bị.

Trong ví dụ đơn giản này, giả sử không có đầu vào, nên \(\boldsymbol{u}_n=0\).

Để xem ví dụ có thêm hạng tử đầu vào, tham khảo trang Phương trình ngoại suy trạng thái của hướng dẫn trực tuyến hoặc Ví dụ 10 được giải đầy đủ trong sách.

Mỗi phép đo và mỗi ước lượng trong Bộ lọc Kalman đều đi kèm thông tin về độ không đảm bảo. Sau khi dự đoán trạng thái tiếp theo, ta cũng nên hỏi: dự đoán này chính xác đến mức nào?

Độ không đảm bảo bình phương của ước lượng trạng thái hiện tại được biểu diễn bằng ma trận hiệp phương sai:

\[ \boldsymbol{P}_{0,0}=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

Tuy nhiên, hiệp phương sai của dự đoán không được tính theo công thức:

\[ \textcolor{red}{\xcancel{\textcolor{black}{ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0} }}} \]

Điều này là vì \(\boldsymbol{P}\) là ma trận hiệp phương sai, và phương sai cùng hiệp phương sai liên quan đến các hạng tử bình phương.

Phương trình ngoại suy hiệp phương sai (không tính nhiễu quá trình) được cho bởi:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T \]

Bạn có thể xem phần diễn giải đầy đủ trong phần Phương trình ngoại suy hiệp phương sai của hướng dẫn trực tuyến.

Đối với ví dụ của chúng ta:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\1.25&0.25\\\end{matrix}\right]=\left[\begin{matrix}\colorbox{yellow}{$22.25$}&1.25\\1.25&\colorbox{yellow}{$0.25$}\\\end{matrix}\right] $$

Hãy xem các phần tử trên đường chéo chính của ma trận hiệp phương sai.

Phương sai vận tốc \(\sigma_v^2\) vẫn là \(0.25 \, m^2/s^2\). Nó không đổi vì mô hình động giả định vận tốc không đổi.

Ngược lại, phương sai khoảng cách \(\sigma_r^2\) tăng từ \(16m^2\) lên \(22.25m^2\). Điều này phản ánh thực tế: độ không đảm bảo về vận tốc làm tăng độ không đảm bảo về khoảng cách theo thời gian.

Như đã đề cập, giả định động lực vận tốc không đổi không hoàn toàn chính xác. Trong thực tế, vận tốc máy bay có thể bị ảnh hưởng bởi các yếu tố bên ngoài và chưa biết, như gió. Do đó, độ không đảm bảo dự đoán thực tế cao hơn so với dự đoán của mô hình đơn giản.

Những ảnh hưởng khó dự đoán này được gọi là nhiễu quá trình (process noise) và được ký hiệu \(\boldsymbol{Q}\). Để xét tới các hiệu ứng này, ta cộng \(\boldsymbol{Q}\) vào phương trình hiệp phương sai dự đoán:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]

Để xây dựng trực giác về ảnh hưởng của nhiễu quá trình tới hiệu năng Bộ lọc Kalman, xem Ví dụ 6 trong hướng dẫn trực tuyến.

Giả sử độ lệch chuẩn của gia tốc ngẫu nhiên là \(\sigma_a=0.2m/s^2\). Điều này phản ánh bất định trong gia tốc ngẫu nhiên của máy bay do ảnh hưởng môi trường khó dự đoán.

Khi đó, phương sai gia tốc ngẫu nhiên \(\sigma_a^2=0.04m^2/s^4\).

Với ví dụ này, ma trận nhiễu quá trình được cho bởi:

$$ \boldsymbol{Q} = \left[\begin{matrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{matrix}\right] \sigma_a^2 $$

Với \(\Delta t=5\mathrm{s}\) và \(\sigma_a^2=0.04\,\mathrm{m}^2/\mathrm{s}^4\), ta có:

$$ \boldsymbol{Q}=\left[\begin{matrix}\frac{625}{4}&\frac{125}{2}\\[0.5em] \frac{125}{2}&25\\\end{matrix}\right]0.04=\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right] $$

Diễn giải cách suy ra ma trận nhiễu quá trình được trình bày trong Mục 8.2.2 của sách.

Sau khi cộng thêm nhiễu quá trình, độ không đảm bảo bình phương của dự đoán là:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T+\boldsymbol{Q}\ =\left[\begin{matrix}22.25&1.25\\1.25&0.25\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]\ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] $$

Tóm tắt vòng lặp 0

• Khởi tạo
  Ta khởi tạo Bộ lọc Kalman bằng cách dùng phép đo đầu tiên làm ước lượng trạng thái ban đầu \( {\hat{\boldsymbol{x}}}_{0,0} \), và dùng hiệp phương sai đo lường làm hiệp phương sai trạng thái ban đầu \(\boldsymbol{P}_{0,0}\).
  Lưu ý rằng điều này chỉ có thể thực hiện trong giai đoạn khởi tạo.
• Dự đoán
  Ta đã dự đoán trạng thái và độ không đảm bảo của nó tại bước thời gian tiếp theo, khi ra-đa quay lại máy bay. Các phương trình dự đoán của Bộ lọc Kalman là:
  
  
  Phương trình ngoại suy trạng thái (State Extrapolation Equation)
  \[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]
  Phương trình ngoại suy hiệp phương sai (Covariance Extrapolation Equation)
  \[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]
  trong đó:
  
  • \(\hat{\boldsymbol{x}}_{n,n}\) là vector trạng thái hệ thống được ước lượng tại bước thời gian \(n\)
  • \(\hat{\boldsymbol{x}}_{n+1,n}\) là vector trạng thái hệ thống dự đoán cho bước thời gian \(n+1\), được tính bằng thông tin sẵn có tại thời điểm \(n\)
  • \(\boldsymbol{u}_n\) là biến điều khiển hoặc biến đầu vào, biểu diễn các tín hiệu bên ngoài đã biết đưa vào hệ thống
  • \(\boldsymbol{F}\) là ma trận chuyển trạng thái
  • \(\boldsymbol{G}\) là ma trận đầu vào (điều khiển) hoặc ma trận chuyển tiếp đầu vào, ánh xạ các đầu vào tới các biến trạng thái
  • \(\boldsymbol{P}_{n,n}\) là ma trận hiệp phương sai (độ không đảm bảo bình phương) của trạng thái hiện tại
  • \(\boldsymbol{P}_{n+1,n}\) là ma trận hiệp phương sai (độ không đảm bảo bình phương) của trạng thái dự đoán
  • \(\boldsymbol{Q}\) là ma trận nhiễu quá trình

Vòng lặp 1

Cập nhật bộ lọc

Giả sử phép đo thứ hai tại \(t_1\):

\[ \boldsymbol{z}_1=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] \]

Do có một xung nhiễu mạnh trong phép đo này, tỷ lệ tín hiệu trên nhiễu thấp hơn đáng kể so với phép đo đầu tiên. Do đó, độ không đảm bảo của phép đo thứ hai cao hơn.

Giả sử độ lệch chuẩn của phép đo khoảng cách là \(6m\) và độ lệch chuẩn của phép đo vận tốc là \(1.5m/s\). Ma trận hiệp phương sai đo lường tương ứng là:

\[ \boldsymbol{R}_1=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Ta muốn ước lượng trạng thái hệ thống hiện tại \(\hat{\boldsymbol{x}}_{1,1}\). Tại thời điểm \(t_1\), chúng ta có hai thông tin:

• Trạng thái dự đoán \(\hat{\boldsymbol{x}}_{1,0}\) (tính từ bước trước), và
• Phép đo mới \(\boldsymbol{z}_1\)

Chúng ta nên tin vào cái nào?

Theo trực giác, ta có thể muốn dùng phép đo làm ước lượng hiện tại, tức là \(\hat{\boldsymbol{x}}_{1,1}=\boldsymbol{z}_1\), vì nó cập nhật hơn so với dự đoán.

Mặt khác, phép đo cũng nhiễu hơn. Nếu so sánh các phần tử trên đường chéo chính của hiệp phương sai dự đoán \(\boldsymbol{P}_{1,0}\) với hiệp phương sai đo lường \(\boldsymbol{R}_1\), ta thấy độ không đảm bảo của dự đoán nhỏ hơn độ không đảm bảo của phép đo:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right] \]

Vậy có lẽ ta nên bỏ qua phép đo mới và giữ dự đoán, tức là \(\hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}\)?

Trong trường hợp này, ta sẽ mất thông tin mới do phép đo hiện tại cung cấp.

Ý tưởng then chốt của Bộ lọc Kalman là chúng ta không làm cách nào trong hai cách đó. Thay vào đó, ta kết hợp dự đoán và phép đo, gán trọng số lớn hơn cho thành phần có độ không đảm bảo thấp hơn.

Lời giải là trung bình có trọng số giữa phép đo và dự đoán:

\[ \hat{x}_{1,1}=K_1 z_1\ +\ \left({1-\ K}_1\right){\hat{x}}_{1,0}, \quad 0\leq K_1 \leq 1 \]

Ở đây, trọng số \(K_1\) chính là Hệ số Kalman (Kalman Gain). Nó quyết định mức độ trọng số dành cho phép đo so với dự đoán theo cách tối thiểu hóa độ không đảm bảo của ước lượng. Đây là điều làm cho Bộ lọc Kalman trở thành bộ lọc tối ưu (miễn là hệ thống và nhiễu tuân theo các giả định của mô hình).

Tôi sẽ giới thiệu phương trình hệ số Kalman (Kalman Gain) ngay sau đây, nhưng trước hết hãy tập trung vào Phương trình cập nhật trạng thái. Ở dạng ma trận, phương trình này được viết như sau:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + (\boldsymbol{I} - \boldsymbol{K}_1)\hat{\boldsymbol{x}}_{1,0} \]

trong đó \(\boldsymbol{I}\) là ma trận đơn vị (ma trận vuông có các phần tử 1 trên đường chéo chính và 0 ở các vị trí khác).

Ta có thể viết lại phương trình này:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + \hat{\boldsymbol{x}}_{1,0} - \boldsymbol{K}_1\hat{\boldsymbol{x}}_{1,0}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}) \]

Dạng này cho thấy trạng thái sau cập nhật bằng dự đoán \(\hat{\boldsymbol{x}}_{1,0}\) cộng với một hạng tử hiệu chỉnh \(\boldsymbol{K}_1\left(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\right)\).

Hạng tử hiệu chỉnh tỉ lệ với độ chênh giữa phép đo và dự đoán \(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\), được gọi là phần dư đổi mới (innovation) hoặc phần dư (residual).

Trong ví dụ của chúng ta, cả trạng thái hệ thống và phép đo đều là các vector biểu diễn cùng đại lượng vật lý (khoảng cách và vận tốc). Do đó, ta có thể trừ trực tiếp \(\hat{\boldsymbol{x}}_{1,0}\) khỏi \(\boldsymbol{z}_1\).

Tuy nhiên, điều này không phải lúc nào cũng đúng. Nói chung, phép đo và trạng thái hệ thống có thể thuộc các miền vật lý khác nhau. Ví dụ, một nhiệt kế số đo tín hiệu điện, trong khi trạng thái hệ thống là nhiệt độ.

Vì lý do đó, trạng thái dự đoán trước hết phải được biến đổi sang miền đo lường:

\[ \boldsymbol{H} \hat{\boldsymbol{x}}_{1,0} \]

Ma trận \(\boldsymbol{H}\) được gọi là ma trận quan sát (hoặc ma trận đo lường). Nó ánh xạ các biến trạng thái sang các đại lượng được đo thực tế.

Trong ví dụ này, ma trận quan sát đơn giản chính là ma trận đơn vị:

\[ \boldsymbol{H}=\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]=\boldsymbol{I} \]

Để tìm hiểu thêm về ma trận quan sát, xem phần Phương trình đo lường của hướng dẫn trực tuyến và Ví dụ 9 và 10 trong sách.

Giờ ta có thể viết lại phương trình cập nhật trạng thái như sau:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

Phần dư đổi mới \(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}\) biểu diễn thông tin mới.

Hệ số Kalman xác định mức độ mà thông tin mới này sẽ làm thay đổi trạng thái dự đoán, tức là mức độ chúng ta hiệu chỉnh dự đoán.

Trường hợp một chiều

Trong trường hợp một chiều, Hệ số Kalman (Kalman Gain) được cho bởi:

\[ K_n=\frac{p_{n,\ n-1}}{p_{n,\ n-1}+r_n} \]

trong đó:

• \(p_{n,\ n-1}\) là phương sai trạng thái dự đoán
• \(r_n\) là phương sai đo lường

Hệ số Kalman được chọn để tối thiểu hóa phương sai của ước lượng sau cập nhật \(p_{n,n}\), đó là lý do Bộ lọc Kalman tối ưu.

Để xây dựng trực giác và xem diễn giải đầy đủ trong trường hợp một chiều, xem phần Bộ lọc Kalman một chiều trong hướng dẫn trực tuyến.

Trường hợp đa biến

Với Bộ lọc Kalman đa biến, hệ số Kalman là một ma trận và được cho bởi:

\[ \boldsymbol{K}_n=\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1} \]

Để xem phần diễn giải của phương trình Hệ số Kalman đa biến, xem phần Hệ số Kalman trong hướng dẫn trực tuyến.

Hãy tính Hệ số Kalman cho \(t_1\):

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1} \]

Trong ví dụ này, \(\boldsymbol{H}=\boldsymbol{I}\) và \(\boldsymbol{H}^T=\boldsymbol{I}\).

Thay các ma trận vào:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right], \quad \boldsymbol{R}_1=\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}64.5&3.75\\3.75&3.5\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}0.0165&-0.0177\\-0.0177&0.3047\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

Ước lượng trạng thái sau cập nhật là:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

Trong ví dụ này, \(\boldsymbol{H}=\boldsymbol{I}\), nên phần đổi mới/phần dư đo lường (innovation/residual) đơn giản là:

\[ \boldsymbol{z}_1 - \boldsymbol{I}\hat{\boldsymbol{x}}_{1,0}=\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] - \left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]=\left[\begin{matrix}20\\2\\\end{matrix}\right] \]

Áp dụng hiệu chỉnh:

\[ \boldsymbol{K}_1\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right] \]

Cuối cùng:

\[ \hat{\boldsymbol{x}}_{1,1}=\left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]+\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right]=\left[\begin{matrix}11{,}009.37\\201.43\\\end{matrix}\right] \]

Sau khi ước lượng trạng thái hiện tại, chúng ta cũng muốn định lượng độ không đảm bảo của ước lượng đó.

Trường hợp một chiều

Trong trường hợp một chiều, Phương trình cập nhật hiệp phương sai là:

\[ p_{n,n}=(1-K_n)p_{n,\ n-1} \]

Để xem phần diễn giải, xem phần Bộ lọc Kalman một chiều trong hướng dẫn trực tuyến.

Trường hợp đa biến

Dạng Joseph

Với Bộ lọc Kalman đa biến, phương trình cập nhật hiệp phương sai thường được viết ở dạng ổn định số, gọi là dạng Joseph, do Peter Joseph giới thiệu.

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1}(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]

trong đó:

• \(\boldsymbol{P}_{n,n}\) là hiệp phương sai của ước lượng trạng thái đã cập nhật (hậu nghiệm)
• \(\boldsymbol{P}_{n,n-1}\) là hiệp phương sai của ước lượng trạng thái dự đoán (tiền nghiệm)
• \(\boldsymbol{K}_n\) là Hệ số Kalman (Kalman Gain)
• \(\boldsymbol{H}\) là ma trận quan sát (đo lường)
• \(\boldsymbol{R}_n\) là ma trận hiệp phương sai nhiễu đo lường
• \(\boldsymbol{I}\) là ma trận đơn vị (ma trận vuông có các phần tử 1 trên đường chéo chính và 0 ở các vị trí khác)

Để xem phần diễn giải, xem phần Phương trình cập nhật hiệp phương sai trong hướng dẫn trực tuyến.

Dạng rút gọn

Trong tài liệu, bạn cũng thường thấy dạng cập nhật hiệp phương sai rút gọn:

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1} \]

Để xem phần diễn giải, xem phần Phương trình cập nhật hiệp phương sai dạng rút gọn.

Cả hai dạng cho kết quả giống nhau trong số học chính xác. Tuy nhiên, với triển khai trên máy tính, dạng Joseph thường được ưa dùng vì ổn định số hơn.

Riêng ví dụ này, ta dùng phương trình cập nhật hiệp phương sai dạng rút gọn:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1\boldsymbol{H})\boldsymbol{P}_{1,0} \]

Trong ví dụ này, \(\boldsymbol{H}=\boldsymbol{I}\), nên:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1)\boldsymbol{P}_{1,0} \]

Bây giờ thay các ma trận vào:

\[ \boldsymbol{P}_{1,1}=\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right] - \left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\right)\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] \]

\[ =\left[\begin{matrix}0.5952&-0.6377\\-0.0399&0.6856\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]=\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right] \]

Phân tích kết quả

Độ không đảm bảo của ước lượng sau cập nhật thấp hơn cả độ không đảm bảo dự đoán và độ không đảm bảo đo lường:

\[ \boldsymbol{P}_{1,1}=\left[\begin{matrix}\colorbox{yellow}{$14.57$}&1.43\\1.43&\colorbox{yellow}{$0.71$}\\\end{matrix}\right]\ \ \ \ \ \ \boldsymbol{P}_{1,0}=\ \left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right]\ \ \ \ \ \boldsymbol{R}_\mathbf{1}=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Bằng cách kết hợp phép đo với dự đoán và gán trọng số bằng hệ số Kalman, chúng ta thu được một ước lượng có độ không đảm bảo thấp hơn.

Việc bổ sung thông tin mới, ngay cả khi có độ không đảm bảo cao, luôn làm giảm độ không đảm bảo của ước lượng. Xem chương Hợp nhất cảm biến (Sensor Fusion) trong sách và Phụ lục G và H để xem chứng minh toán học. Về mặt lý thuyết, không nên bỏ qua các phép đo mới.

Tuy nhiên, trong thực tế, đôi khi cần loại bỏ một số phép đo. Xem chương Xử lý ngoại lai (Outlier Treatment) trong sách để tham khảo các phương pháp xử lý phép đo không đáng tin cậy.

Dự đoán

Bước dự đoán của Vòng lặp 1 (từ \( t_1 \) đến \( t_2 \)) giống hệt bước dự đoán của Vòng lặp 0 (từ \( t_0 \) đến \( t_1 \)), ngoại trừ việc giờ đây ta bắt đầu từ ước lượng đã cập nhật \(\hat{\boldsymbol{x}}_{1,1}\) và \(\boldsymbol{P}_{1,1}\).

Dự đoán trạng thái

\[ \hat{\boldsymbol{x}}_{2,1}=\boldsymbol{F}\hat{\boldsymbol{x}}_{1,1} \]

\[ \hat{\boldsymbol{x}}_{2,1}=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}11,009.37\\201.43\\\end{matrix}\right]=\left[\begin{matrix}12,016.5\\201.43\\\end{matrix}\right] \]

Dự đoán hiệp phương sai

\[ \boldsymbol{P}_{2,1}=\boldsymbol{F}\boldsymbol{P}_{1,1}\boldsymbol{F}^\top + \boldsymbol{Q} \]

\[ \boldsymbol{P}_{2,1}=\ \left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]=\left[\begin{matrix}52.86&7.47\\7.47&1.71\\\end{matrix}\right] \]

Lưu ý rằng cả hai phương sai lại tăng trong bước dự đoán. Điều này xảy ra vì khi thời gian trôi qua mà không có phép đo mới, độ không đảm bảo tự nhiên tăng lên. Đặc biệt, độ không đảm bảo về vận tốc gây thêm độ không đảm bảo về khoảng cách, do đó phương sai khoảng cách tăng nhanh hơn phương sai vận tốc.

Tóm tắt vòng lặp 1

• Cập nhật
  
  • Ta ước lượng trạng thái hệ thống hiện tại \(\hat{\boldsymbol{x}}_{1,1}\) như một tổ hợp có trọng số của trạng thái dự đoán \(\hat{\boldsymbol{x}}_{1,0}\) và phép đo \(\boldsymbol{z}_1\).
    Trọng số được xác định bởi Hệ số Kalman \(K_1\). Hệ số Kalman được tính từ hiệp phương sai trạng thái dự đoán \(\boldsymbol{P}_{1,0}\) và hiệp phương sai đo lường \(\boldsymbol{R}_1\), và nó tối thiểu hóa độ không đảm bảo của ước lượng sau cập nhật \(\boldsymbol{P}_{1,1}\).
  • Các phương trình cập nhật của Bộ lọc Kalman là:
    
    
    Phương trình cập nhật trạng thái (State Update Equation)
    \[ \hat{\boldsymbol{x}}_{n,n}=\hat{\boldsymbol{x}}_{n,n-1}+\boldsymbol{K}_n\left(\boldsymbol{z}_n\ -\ \boldsymbol{H}\hat{\boldsymbol{x}}_{n,n-1}\right) \]
    Phương trình cập nhật hiệp phương sai (Dạng Joseph)
    \[ \boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)^T+\boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]
    Hoặc dạng rút gọn của nó
    \[\boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\]
    Phương trình hệ số Kalman (Kalman Gain)
    \[ \boldsymbol{K}_n=\ \boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1}\]
  trong đó:
  
  • \( \hat{\boldsymbol{x}}_{n,n} \) là ước lượng trạng thái đã cập nhật tại bước thời gian n
  • \( \hat{\boldsymbol{x}}_{n,n-1} \) là trạng thái dự đoán tại bước thời gian n, được tính bằng thông tin sẵn có tại thời điểm n-1
  • \( \boldsymbol{z}_n \) là vector đo lường
  • \( \boldsymbol{P}_{n,n} \) là hiệp phương sai của ước lượng trạng thái đã cập nhật
  • \( \boldsymbol{P}_{n,n-1} \) là hiệp phương sai của ước lượng trạng thái dự đoán
  • \( \boldsymbol{K}_n \) là Hệ số Kalman (Kalman Gain)
  • \( \boldsymbol{H} \) là ma trận quan sát (đo lường)
  • \( \boldsymbol{R}_n \) là ma trận hiệp phương sai nhiễu đo lường
  • \( \boldsymbol{I} \) là ma trận đơn vị
• Dự đoán
  Bước dự đoán trong Vòng lặp 1 giống với Vòng lặp 0.
  Ta lan truyền ước lượng trạng thái hiện tại và hiệp phương sai của nó tới bước thời gian tiếp theo, khi ra-đa quay lại máy bay, bằng mô hình chuyển trạng thái.

Tóm tắt ví dụ