a. Mô hình Quá trình (Process Model hay State Transition Model)#

Phương trình này mô tả cách trạng thái của hệ thống $\mathbf{x}_k \in \mathbb{R}^n$ tại thời điểm (rời rạc) $k$ tiến triển từ trạng thái $\mathbf{x}_{k\!-\!1}$ ở thời điểm trước đó $k-1$:

Trong đó:

• $\mathbf{x}_k$: Vector trạng thái tại thời điểm $k$. Ví dụ: vị trí, vận tốc, gia tốc của một đối tượng.
• $\mathbf{A}_k$: Ma trận chuyển đổi trạng thái (state transition matrix) từ $k-1$ sang $k$. Nó mô tả động lực học nội tại của hệ thống.
• $\mathbf{u}_k$: Vector đầu vào điều khiển (control input vector) tại thời điểm $k$ (nếu có). Ví dụ: lực tác động lên một robot.
• $\mathbf{B}_k$: Ma trận điều khiển đầu vào (control input matrix). Nó liên kết đầu vào điều khiển với trạng thái.
• $\mathbf{w}_{k-1}$: Vector nhiễu quá trình (process noise vector) tại $k-1$. Đây là một vector ngẫu nhiên Gauss đa biến, giả định là nhiễu trắng với kỳ vọng bằng không và ma trận hiệp phương sai $\mathbf{Q}_k$: $\mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)$ Nhiễu quá trình $\mathbf{w}$ thể hiện sự không chắc chắn trong mô hình động của chúng ta, hoặc các yếu tố ngoại cảnh không được mô hình hóa. Ví dụ, nếu theo dõi một chiếc xe, $\mathbf{w}$ có thể đại diện cho những thay đổi gia tốc không lường trước do gió, mặt đường không bằng phẳng, hoặc sai số trong mô hình động lực học của xe.

b. Mô hình Đo lường (Measurement Model)#

Phương trình này mô tả mối quan hệ giữa trạng thái thực $\mathbf{x}_k$ và phép đo $\mathbf{z}_k \in \mathbb{R}^m$ mà chúng ta thu được từ các cảm biến tại thời điểm $k$:

$\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k$

Trong đó:

• $\mathbf{z}_k$: Vector đo lường tại thời điểm $k$. Ví dụ: vị trí đo được từ GPS, góc đo từ radar.
• $\mathbf{H}_k$: Ma trận đo lường (measurement matrix hoặc observation matrix). Nó liên kết trạng thái thực với không gian đo lường.
• $\mathbf{v}_k$: Vector nhiễu đo lường (measurement noise vector) tại $k$. Đây cũng là một vector ngẫu nhiên Gauss đa biến, giả định là nhiễu trắng với kỳ vọng bằng không và ma trận hiệp phương sai $\mathbf{R}_k$: $\mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)$ Nhiễu đo lường $\mathbf{v}$ thể hiện sự không chính xác và nhiễu cố hữu của các cảm biến.

Các giả định quan trọng:

• Nhiễu quá trình $\mathbf{w}_k$ và nhiễu đo lường $\mathbf{v}_k$ là độc lập thống kê với nhau.
• Chúng cũng độc lập với trạng thái ban đầu $\mathbf{x}_0$.
• Chúng là các chuỗi nhiễu trắng (uncorrelated in time).

a. Dự đoán trạng thái tiên nghiệm ($\hat{\mathbf{x}}_{k|k-1}$)#

Chúng ta sử dụng mô hình quá trình để dự đoán trạng thái:

$\hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_k \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k$

Chứng minh: Lấy kỳ vọng của phương trình mô hình quá trình $\mathbf{x}_k = \mathbf{A}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_{k-1}$, có điều kiện là tất cả các phép đo đến $k-1$ (ký hiệu $Z_{k-1}$): $E[\mathbf{x}_k | Z_{k-1}] = E[\mathbf{A}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_{k-1} | Z_{k-1}]$ $\hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_k E[\mathbf{x}_{k-1} | Z_{k-1}] + \mathbf{B}_k \mathbf{u}_k + E[\mathbf{w}_{k-1} | Z_{k-1}]$ Vì $\mathbf{w}_{k-1}$ là nhiễu trắng có kỳ vọng bằng 0 và độc lập với các phép đo quá khứ, $E[\mathbf{w}_{k-1} | Z_{k-1}] = E[\mathbf{w}_{k-1}] = \mathbf{0}$. Do đó: $\hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_k \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k$.

b. Dự đoán hiệp phương sai sai số tiên nghiệm ($\mathbf{P}_{k|k-1}$)#

Sai số dự đoán tiên nghiệm là: $\mathbf{e}_{k|k-1} = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1}$. $\mathbf{e}_{k|k-1} = (\mathbf{A}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_{k-1}) - (\mathbf{A}_k \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k)$ $\mathbf{e}_{k|k-1} = \mathbf{A}_k (\mathbf{x}_{k-1} - \hat{\mathbf{x}}_{k-1|k-1}) + \mathbf{w}_{k-1} = \mathbf{A}_k \mathbf{e}_{k-1|k-1} + \mathbf{w}_{k-1}$.

Hiệp phương sai: $\mathbf{P}_{k|k-1} = E[\mathbf{e}_{k|k-1} \mathbf{e}_{k|k-1}^T]$ $= E[(\mathbf{A}_k \mathbf{e}_{k-1|k-1} + \mathbf{w}_{k-1})(\mathbf{A}_k \mathbf{e}_{k-1|k-1} + \mathbf{w}_{k-1})^T]$ $= E[\mathbf{A}_k \mathbf{e}_{k-1|k-1} \mathbf{e}_{k-1|k-1}^T \mathbf{A}_k^T + \mathbf{A}_k \mathbf{e}_{k-1|k-1} \mathbf{w}_{k-1}^T + \mathbf{w}_{k-1} \mathbf{e}_{k-1|k-1}^T \mathbf{A}_k^T + \mathbf{w}_{k-1} \mathbf{w}_{k-1}^T]$.

Vì sai số $\mathbf{e}_{k-1|k-1}$ và nhiễu quá trình $\mathbf{w}_{k-1}$ là độc lập và có kỳ vọng bằng không, các số hạng chéo (cross terms) sẽ bằng không khi lấy kỳ vọng. $E[\mathbf{A}_k \mathbf{e}_{k-1|k-1} \mathbf{w}_{k-1}^T] = \mathbf{A}_k E[\mathbf{e}_{k-1|k-1}] E[\mathbf{w}_{k-1}^T] = \mathbf{0}$ (vì $E[\mathbf{e}_{k-1|k-1}] = \mathbf{0}$ nếu ước lượng là không chệch). Do đó: $\mathbf{P}_{k|k-1} = \mathbf{A}_k E[\mathbf{e}_{k-1|k-1} \mathbf{e}_{k-1|k-1}^T] \mathbf{A}_k^T + E[\mathbf{w}_{k-1} \mathbf{w}_{k-1}^T]$

$\mathbf{P}_{k|k-1} = \mathbf{A}_k \mathbf{P}_{k-1|k-1} \mathbf{A}_k^T + \mathbf{Q}_{k-1}$

(Lưu ý: một số tài liệu dùng $\mathbf{Q}_k$ thay vì $\mathbf{Q}_{k-1}$, tùy thuộc vào quy ước chỉ số thời gian của nhiễu).

a. Tính toán Kalman Gain ($\mathbf{K}_k$)#

Mục tiêu là chọn $\mathbf{K}_k$ sao cho phương sai của sai số ước lượng hậu nghiệm $\mathbf{P}_{k|k}$ là nhỏ nhất. Điều này tương đương với việc tối thiểu hóa trace (tổng các phần tử trên đường chéo chính) của $\mathbf{P}_{k|k}$.

Sai số hậu nghiệm: $\mathbf{e}_{k|k} = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k}$

$\mathbf{e}_{k|k} = \mathbf{x}_k - [\hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1})]$

Thay $\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k$:

$\mathbf{e}_{k|k} = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1} - \mathbf{K}_k (\mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1})$

$\mathbf{e}_{k|k} = (\mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1}) - \mathbf{K}_k \mathbf{H}_k (\mathbf{x}_k - \hat{\mathbf{x}}_{k|k-1}) - \mathbf{K}_k \mathbf{v}_k$

$\mathbf{e}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{e}_{k|k-1} - \mathbf{K}_k \mathbf{v}_k$

Hiệp phương sai sai số hậu nghiệm: $\mathbf{P}_{k|k} = E[\mathbf{e}_{k|k} \mathbf{e}_{k|k}^T]$

$\mathbf{P}_{k|k} = E[ ((\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{e}_{k|k-1} - \mathbf{K}_k \mathbf{v}_k) ((\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{e}_{k|k-1} - \mathbf{K}_k \mathbf{v}_k)^T ]$

Vì $\mathbf{e}_{k|k-1}$ (dựa trên $\mathbf{w}$ đến $k-1$) và $\mathbf{v}_k$ (nhiễu đo lường tại $k$) là độc lập và có kỳ vọng bằng không, các số hạng chéo sẽ bằng không:

$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) E[\mathbf{e}_{k|k-1} \mathbf{e}_{k|k-1}^T] (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k E[\mathbf{v}_k \mathbf{v}_k^T] \mathbf{K}_k^T$

$\Leftrightarrow \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T$

Đây là dạng Joseph (Joseph form) của phương trình cập nhật hiệp phương sai, nó ổn định hơn về mặt số học.

Để tìm $\mathbf{K}_k$ tối ưu, ta lấy đạo hàm của trace của $\mathbf{P}_{k|k}$ theo $\mathbf{K}_k$ và đặt bằng 0.

$\text{trace}(\mathbf{P}_{k|k}) = \text{trace}[(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T]$

$\Leftrightarrow \text{trace}(\mathbf{P}_{k|k}) = \text{trace}[\mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} - \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T + \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T]$

Lấy đạo hàm theo $\mathbf{K}_k$ (sử dụng các quy tắc đạo hàm ma trận) ta có:

$\frac{\partial \text{trace}(\mathbf{P}_{k|k})}{\partial \mathbf{K}_k} = -2 \mathbf{P}_{k|k-1} \mathbf{H}_k^T + 2 \mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k) = \mathbf{0}$

Giải phương trình này cho $\mathbf{K}_k$ ta được:

$2 \mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k) = 2 \mathbf{P}_{k|k-1} \mathbf{H}_k^T$

$\Rightarrow \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k)^{-1}$

Ma trận $(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k)$ được gọi là ma trận hiệp phương sai innovation (innovation covariance matrix), ký hiệu $\mathbf{S}_k$.

Vậy:

b. Cập nhật ước lượng trạng thái hậu nghiệm ($\hat{\mathbf{x}}_{k|k}$)#

Như đã nêu ở trên:

$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1})$

c. Cập nhật hiệp phương sai sai số hậu nghiệm ($\mathbf{P}_{k|k}$)#

Thay $\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}$ vào dạng Joseph của $\mathbf{P}_{k|k}$ ở trên là một cách. Một dạng phổ biến và đơn giản hơn (dù có thể kém ổn định số học hơn dạng Joseph trong một số trường hợp): $\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T$ $= \mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} - \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T + \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T$ $= \mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1} - \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{K}_k^T + \mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k) \mathbf{K}_k^T$ Từ định nghĩa $\mathbf{K}_k$, ta có $\mathbf{K}_k (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k) = \mathbf{P}_{k|k-1} \mathbf{H}_k^T$. Vậy $(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k) \mathbf{K}_k^T = \mathbf{H}_k \mathbf{P}_{k|k-1}$. Thay vào: $\mathbf{P}_{k|k} = \mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k|k-1}$ $= (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}$. Đây là dạng đơn giản và thường được sử dụng.