bộ lọc kalman với các cảm biến dự phòng

Giả sử tôi có một robot có hai cảm biến vị trí 3D dựa trên các nguyên tắc vật lý khác nhau và tôi muốn chạy chúng thông qua bộ lọc Kalman. Tôi xây dựng một ma trận quan sát hai đại diện cho hai cảm biến của tôi bằng cách ghép dọc hai ma trận danh tính.

$H = \begin{bmatrix} 1&0&0\\0&1&0\\0&0&1\\1&0&0\\0&1&0\\0&0&1 \end{bmatrix}$ $\hspace{20pt}$ $\overrightarrow x = \begin{bmatrix} x\\y\\z \end{bmatrix}$

vậy nên

$H \overrightarrow x = \begin{bmatrix} x\\y\\z\\x\\y\\z \end{bmatrix}$

đại diện cho cả hai cảm biến đọc vị trí chính xác của robot. Làm cho ý nghĩa cho đến nay. Vấn đề xảy ra khi tôi tính hiệp phương sai đổi mới

$S_k = R + HP_{k|k-1}H^T$

Từ

$H H^T = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 1 \\ 0 & 1 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix}$

sau đó, bất kể $P$ là gì , tôi sẽ kết thúc với những đổi mới $x$ từ cảm biến đầu tiên có tương quan với đổi mới $z$ từ thứ hai, điều này có vẻ sai theo trực giác, nếu tôi diễn giải điều này đúng.

Tiếp tục từ đây, ma trận của tôi ( ) kết thúc việc thực hiện một số nội dung khá kỳ lạ (hoán đổi hàng và tương tự) để khi cập nhật hệ thống tĩnh ( ) với số đo không đổi Tôi kết thúc với trạng thái dự đoán .$K = P_{k|k-1} H^T S_k^{-1}$$A = I_3, B = [0]$$\overrightarrow z = [1,0,0]$$\hat x = [0,0,1]$

Nếu tôi tách các cảm biến và cập nhật bộ lọc với từng phép đo riêng biệt, thì và tôi nhận được kết quả hợp lý.$H H^T = I_3$

Tôi nghĩ rằng tôi bối rối về một số điểm kỹ thuật trong một hoặc nhiều bước này. Tôi làm sai ở đâu? Nó không có ý nghĩa để nối theo chiều dọc các ma trận quan sát?

Tôi cho rằng tôi chỉ có thể đặt các khối 3x3 ngoài đường chéo của thành 0, vì tôi biết rằng các cảm biến là độc lập, nhưng có lý thuyết nào gợi ý hoặc kết hợp bước này không?$S_k$

Bạn có chắc chắn về biểu hiện của bạn cho ?$HH^T$

tôi có