Hiểu bộ lọc Kalman với một ví dụ radar đơn giản

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

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

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

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

Chúng ta ký hiệu các vector bằng chữ in đậm thường và các ma trận bằng chữ in đậm hoa.

Vì trạng thái hệ thống bao gồm nhiều 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 (Kalman Filter). 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 (One-Dimensional Kalman Filter section) 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à cách suy luận chúng bằng toán học trình độ phổ thông, cùng với bốn ví dụ đã được giải chi tiết.

Lần lặp 0

Khởi tạo bộ lọc

Trong ví dụ này, chúng ta sẽ sử dụng phép đo đầu tiên để khởi tạo Bộ lọc Kalman (để biết thêm thông tin về các kỹ thuật khởi tạo và tác động của chúng đối với hiệu suất của Bộ lọc Kalman, hãy tham khảo Chương 21 của cuốn sách). Tại thời điểm \(t_0\), radar đo được phạm vi là \(10,000m\) và vận tốc là \(200m/s\). Các phép đo được ký hiệu bằng chữ cái \(\boldsymbol{z}\).
Chúng ta xếp các phép đo vào vectơ đo lường \(\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. Các phép đo bị nhiễu ngẫu nhiên làm sai lệch; do đó, mỗi phép đo là một biến ngẫu nhiên.

Chúng ta có thể tin tưởng 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 với một giá trị độ không đảm bảo đo lường bình phương (đôi khi được 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 Kiến thức Nền tảng Thiết yếu I (Essential Background I section). Để thảo luận chi tiết hơn về độ không đảm bảo đo lường, hãy xem phần Bộ lọc Kalman Một chiều (Kalman Filter in One Dimension section).

Trong các hệ thống radar, độ không đảm bảo đo lường chủ yếu được xác định bởi tỷ lệ giữa cường độ tín hiệu thu được và nhiễu. Tỷ lệ tín hiệu trên nhiễu (SNR) càng cao, phương sai đo lường càng thấp và chúng ta càng tin tưởng vào phép đo.

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

Giả sử độ lệch chuẩn của phép đo phạm vi 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, nên độ không đảm bảo đo lường bình phương (ký hiệu là \( \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 các phương sai, và các phần tử ngoài đường chéo là các 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, chúng ta giả định rằng các sai số trong phép đo phạm vi và vận tốc không liên quan đến nhau, vì vậy các phần tử ngoài đường chéo của ma trận hiệp phương sai đo lường được đặt bằng không.

Để ôn lại về phương sai và độ lệch chuẩn, hãy xem phần Kiến thức Nền tảng Thiết yếu I (Essential Background I section) của hướng dẫn trực tuyến.
Để ôn lại về ma trận hiệp phương sai, hãy xem phần Kiến thức Nền tảng Thiết yếu II (Essential Background II section).

Trong quá trình khởi tạo, thông tin duy nhất chúng ta có là một phép đo đơn lẻ. 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\)). Do đó, chúng ta có thể sử dụng phép đo làm ước tính ban đầu cho trạng thái hệ thống. Điều này chỉ có thể thực hiện được 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 sau đây:

• Chỉ số đầu tiên đề cập đến thời điểm của hệ thống, trong ví dụ này là \(t_0\).
• Chỉ số thứ hai đề cập đến thời điểm ước tính được thực hiện, cũng là \(t_0\).

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

Dự đoán

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

Để ước tính trạng thái hệ thống trong tương lai, chúng ta phải mô tả cách hệ thống tiến triển theo thời gian. Trong ví dụ này, chúng ta giả định một mô hình động lực học 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 \]

(Để biết các ví dụ về mô hình động lực học có gia tốc, hãy tham khảo Chương 9 của cuốn sách.)

Hãy mô tả mô hình động lực học dưới 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ố đầu tiên đề cập đến thời gian của hệ thống, đó là \(t_1\).
• Chỉ số thứ hai đề cập đến thời điểm ước tính được thực hiện, đó là \(t_0\).

Do đó, \( \hat{\boldsymbol{x}}_{1,0} \) là ước tính của chúng ta về trạng thái hệ thống tại thời điểm \(t_1\), được tính toán bằng thông tin có sẵn tại thời điểm \(t_0\). Nói cách khác, đó là một dự đoán về 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 (state transition matrix) và mô tả cách trạng thái hệ thống tiến triển 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 cuốn sách mô tả một phương pháp để mô hình hóa động lực học của bất kỳ hệ thống 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 trạng thái (dự đoán). Nó cho chúng ta biết cách tính toán trạng thái tiếp theo từ trạng thái hiện tại. Nó lấy ước tính trạng thái hiện tại của chúng ta và sử dụng mô hình chuyển động của hệ thống để dự đoán trạng thái tại bước thời gian tiếp theo.

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à một biến đầu vào
• \(\boldsymbol{G}\) là ma trận chuyển đầu vào

Vectơ đầu vào đại diện cho thông tin bổ sung được cung cấp cho Bộ lọc Kalman, chẳng hạn như dữ liệu từ cảm biến gia tốc trên máy bay.

Trong ví dụ đơn giản này, chúng ta giả định không có đầu vào, vì vậy \(\boldsymbol{u}_n=0\).

Đối với ví dụ bao gồm một số hạng đầu vào, hãy xem 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 cuốn sách.

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

Độ bất định bình phương của ước tính trạng thái hiện tại được thể hiệ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 dự đoán không được tính bằng:

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

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

Phương trình ngoại suy hiệp phương sai (không bao gồm nhiễu quá trình) được đưa ra bởi:

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

Bạn có thể tìm thấy phần dẫn giải đầy đủ trong mục 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 nhìn vào đườ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 thay đổi vì mô hình động lực giả định vận tốc không đổi.

Ngược lại, phương sai phạm vi (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ế rằng độ bất định trong vận tốc dẫn đến sự gia tăng độ bất định trong phạm vi theo thời gian.

Như đã lưu ý trước đó, giả định về động lực học vận tốc không đổi không hoàn toàn chính xác. Trên thực tế, vận tốc của máy bay có thể bị ảnh hưởng bởi các yếu tố bên ngoài và không xác định, chẳng hạn như gió. Kết quả là, độ bất định dự đoán thực tế cao hơn so với những gì mô hình đơn giản dự đoán.

Những ảnh hưởng không thể dự đoán trước này được gọi là nhiễu quá trình (process noise) và được ký hiệu là \(\boldsymbol{Q}\). Để tính đến các hiệu ứng này, chúng ta thêm \(\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}\]

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

Hãy giả sử rằng độ lệch chuẩn của gia tốc ngẫu nhiên là \(\sigma_a=0.2m/s^2\). Điều này thể hiện sự không chắc chắn trong gia tốc máy bay ngẫu nhiên do các tác động môi trường không thể dự đoán trước.

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

Đối với ví dụ của chúng ta, 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\), biểu thức này trở thành:

$$ \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] $$

Cách dẫn xuất ma trận nhiễu quá trình được trình bày trong Mục 8.2.2 của cuốn sách.

Sau khi thêm nhiễu quá trình, bình phương độ không chắc chắn của dự đoán của chúng ta 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
  Chúng ta đã khởi tạo bộ lọc Kalman bằng cách sử dụng phép đo đầu tiên làm ước tính trạng thái ban đầu \( {\hat{\boldsymbol{x}}}_{0,0} \), và hiệp phương sai phép đo 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
  Chúng ta đã dự đoán trạng thái và độ không chắc chắn của nó tại bước thời gian tiếp theo, khi radar quét 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
  \[ {\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
  \[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]

  trong đó:
  
  • \(\hat{\boldsymbol{x}}_{n,n}\) là vectơ trạng thái hệ thống được ước tính tại bước thời gian \(n\)
  • \(\hat{\boldsymbol{x}}_{n+1,n}\) là vectơ trạng thái hệ thống được dự đoán cho bước thời gian \(n+1\), được tính toán bằng cách sử dụng thông tin có sẵn tại thời điểm \(n\)
  • \(\boldsymbol{u}_n\) là một biến điều khiển hoặc biến đầu vào, thể hiện các đầu vào bên ngoài đã biết cho hệ thống
  • \(\boldsymbol{F}\) là ma trận chuyển đổi trạng thái
  • \(\boldsymbol{G}\) là ma trận đầu vào (điều khiển) hoặc ma trận chuyển đổi đầ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 (bình phương độ không chắc chắn) của trạng thái hiện tại
  • \(\boldsymbol{P}_{n+1,n}\) là ma trận hiệp phương sai (bình phương độ không chắc chắn) của trạng thái được 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 nhiễu tăng đột biến 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. Kết quả là, độ không chắc chắn của phép đo thứ hai cao hơn.

Hãy giả sử rằng độ lệch chuẩn của phép đo phạm vi 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 phép đo tương ứng là:

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

Chúng ta muốn ước tính 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 được dự đoán \(\hat{\boldsymbol{x}}_{1,0}\) (được tính toán từ bước trước), và
• Phép đo mới \(\boldsymbol{z}_1\)

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

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

Mặt khác, phép đo cũng chứa nhiều nhiễu hơn. Nếu chúng ta 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 phép đo \(\boldsymbol{R}_1\), chúng ta thấy rằng độ không chắc chắn của dự đoán nhỏ hơn độ không chắc chắn 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ì vậy, có lẽ chúng ta nên bỏ qua phép đo mới và giữ lại dự đoán, tức là \(\hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}\)?

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

Ý tưởng cốt lõi của bộ lọc Kalman (Kalman Filter) là chúng ta không chọn một trong hai. Thay vào đó, chúng ta kết hợp dự đoán và phép đo, đồng thời ưu tiên (trọng số cao hơn) cho thành phần có độ không đảm bảo thấp hơn.

Giải pháp là một 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à Kalman Gain (Độ lợi Kalman). Nó xác định mức độ ưu tiên 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 tính. Đây là yếu tố khiến bộ lọc Kalman trở thành một 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ẽ sớm giới thiệu phương trình độ lợi Kalman, nhưng trước tiên hãy tập trung vào Phương trình cập nhật trạng thái (State Update Equation). Ở dạng ma trận, nó được viết là:

\[ \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 với các số một trên đường chéo chính và số không ở các vị trí khác).

Hãy 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 đã cập nhật bằng dự đoán \(\hat{\boldsymbol{x}}_{1,0}\) cộng với một số hạng hiệu chỉnh \(\boldsymbol{K}_1\left(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\right)\).

Số hạng hiệu chỉnh tỷ lệ thuận với sự khác biệt giữa phép đo và dự đoán \(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\), được gọi là innovation hoặc residual (phần dư).

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 đại diện cho cùng một đại lượng vật lý (khoảng cách và vận tốc). Do đó, chúng ta có thể trực tiếp lấy \(\boldsymbol{z}_1\) trừ đi \(\hat{\boldsymbol{x}}_{1,0}\).

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

Vì lý do này, trạng thái dự đoán trước tiên phải được chuyể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 thực tế được đo lường.

Trong ví dụ của chúng ta, ma trận quan sát đơn giản là ma trận đơn vị:

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

Để biết thêm thông tin về ma trận quan sát, hãy xem mục Measurement Equation section trong hướng dẫn trực tuyến và Ví dụ 9 và 10 trong cuốn sách.

Bây giờ chúng ta có thể viết lại phương trình cập nhật trạng thái thành:

\[ \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 innovation \(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}\) đại diện cho thông tin mới.

Kalman gain xác định mức độ ảnh hưởng của thông tin mới này lên trạng thái dự đoán, tức là mức độ mạnh mẽ mà 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, Kalman Gain được tính 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 phép đo

Kalman gain được chọn để tối thiểu hóa phương sai của ước tính đã cập nhật \(p_{n,n}\), đó là lý do tại sao bộ lọc Kalman là tối ưu.

Để xây dựng trực giác và xem toàn bộ quá trình suy luận trong trường hợp một chiều, hãy xem mục Kalman Filter in One Dimension trong hướng dẫn trực tuyến.

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

Đối với bộ lọc Kalman đa biến, Kalman gain trở thành một ma trận và được tính 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 cách suy luận Phương trình Kalman Gain đa biến, hãy xem mục Kalman Gain trong hướng dẫn trực tuyến.

Hãy tính Kalman Gain 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ụ của chúng ta, \(\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 tính trạng thái đã 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ụ của chúng ta, \(\boldsymbol{H}=\boldsymbol{I}\), vì vậy phần cải tiến (innovation) đơ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] \]

Bây giờ, hãy á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 tính trạng thái hiện tại, chúng ta cũng muốn định lượng độ không chắc chắn của ước tính đó.

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} \]

Để biết cách suy dẫn, hãy xem phần Bộ lọc Kalman trong một chiều của hướng dẫn trực tuyến.

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

Dạng Joseph

Đối 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ưới dạng ổn định về mặt số học, được 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 tính 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 tính trạng thái đã dự đoán (tiên nghiệm)
• \(\boldsymbol{K}_n\) là Độ lợi 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ị (một ma trận vuông có các số 1 trên đường chéo chính và số 0 ở những vị trí khác)

Để biết cách suy dẫn, hãy xem phần Phương trình Cập nhật Hiệp phương sai của hướng dẫn trực tuyến.

Dạng rút gọn

Trong các 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} \]

Để biết cách suy dẫn, hãy xem phần Phương trình Cập nhật Hiệp phương sai rút gọn.

Cả hai dạng đều cho cùng một kết quả trong số học chính xác. Tuy nhiên, đối với các ứng dụng máy tính, dạng Joseph thường được ưu tiên hơn vì nó ổn định hơn về mặt số học.

Chỉ riêng cho ví dụ này, chúng ta hãy sử dụng phương trình cập nhật hiệp phương sai rút gọn:

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

Trong ví dụ của chúng ta, \(\boldsymbol{H}=\boldsymbol{I}\), vì vậy:

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

Bây giờ thay thế các ma trận:

\[ \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 tính đã 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à cân trọng số chúng sử dụng Kalman gain, chúng ta thu được một ước tính có độ không đảm bảo thấp hơn.

Việc thêm thông tin mới, ngay cả khi nó có độ không đảm bảo cao, luôn làm giảm độ không đảm bảo của ước tính. Xem chương Sensor Fusion trong sách và Phụ lục G và H để xem chứng minh toán học. Từ quan điểm lý thuyết, các phép đo mới không bao giờ nên bị bỏ qua.

Tuy nhiên, trong thực tế, việc loại bỏ một số phép đo nhất định thường là cần thiết. Xem chương Outlier Treatment trong sách để biết các phương pháp thực tế xử lý các phép đo không đáng tin cậy.

Dự đoán

Bước dự đoán của Iteration 1 (từ \( t_1 \) đến \( t_2 \) ) giống hệt với bước dự đoán của Iteration 0 (từ \( t_0 \) đến \( t_1 \) ) ngoại trừ việc bây giờ chúng ta bắt đầu từ ước tính đã 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 đều tăng trở lại trong bước dự đoán. Điều này xảy ra bởi vì khi thời gian trôi qua mà không có phép đo mới, độ không đảm bảo sẽ tăng lên một cách tự nhiên. Cụ thể, độ không đảm bảo về vận tốc gây ra độ không đảm bảo bổ sung về phạm vi, đó là lý do tại sao phương sai phạm vi tăng nhanh hơn phương sai vận tốc.

Tổng kết Iteration 1

• Cập nhật (Update)
  
  • Chúng ta ước tính trạng thái hệ thống hiện tại \(\hat{\boldsymbol{x}}_{1,1}\) như một kế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 Kalman Gain \(K_1\). Kalman Gain được tính toán từ hiệp phương sai trạng thái dự đoán \(\boldsymbol{P}_{1,0}\) và hiệp phương sai phép đo \(\boldsymbol{R}_1\), và nó làm giảm thiểu độ không đảm bảo của ước tính đã cập nhật \(\boldsymbol{P}_{1,1}\).
  • Các phương trình cập nhật Kalman Filter là:

    Phương trình cập nhật trạng thái
    \[ \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 đơn giản hóa 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 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 toán bằng cách sử dụng thông tin có sẵn tại thời điểm n-1
  • \( \boldsymbol{z}_n \) là vectơ đo lường
  • \( \boldsymbol{P}_{n,n} \) là ma trận hiệp phương sai của ước lượng trạng thái đã cập nhật
  • \( \boldsymbol{P}_{n,n-1} \) là ma trận hiệp phương sai của ước lượng trạng thái dự đoán
  • \( \boldsymbol{K}_n \) là độ lợi 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 (Prediction)
  Bước dự đoán trong Lần lặp 1 cũng giống như trong Lần lặp 0.
  Chúng ta truyền trạng thái ước lượng hiện tại và ma trận hiệp phương sai của nó về phía trước tới bước thời gian tiếp theo, khi radar quét lại máy bay, bằng cách sử dụng mô hình chuyển đổi trạng thái.

Tóm tắt ví dụ