Duy trì thuộc tính xác định dương cho hiệp phương sai trong bản cập nhật bộ lọc Kalman không tập trung

Tôi có bộ lọc Kalman không tập trung (UKF) theo dõi trạng thái của robot. Vectơ trạng thái có 12 biến. Mỗi lần tôi thực hiện một bước dự đoán, chức năng chuyển của tôi (tự nhiên) sẽ hoạt động trên toàn bộ trạng thái. Tuy nhiên, các cảm biến của tôi cung cấp các phép đo của các phần khác nhau trong trạng thái của robot, vì vậy tôi có thể nhận được cuộn, cao độ, ngáp và vận tốc tương ứng của chúng trong một phép đo, và sau đó là vận tốc tuyến tính trong một phép đo khác.

Cách tiếp cận của tôi để xử lý vấn đề này cho đến nay chỉ đơn giản là tạo ma trận con cho hiệp phương sai, thực hiện các phương trình cập nhật UKF tiêu chuẩn của tôi và sau đó gắn các giá trị kết quả trở lại vào ma trận hiệp phương sai đầy đủ. Tuy nhiên, sau một vài cập nhật, UKF hét lên với tôi vì đã cố gắng truyền một ma trận không xác định dương vào hàm Cholesky Decysis. Rõ ràng hiệp phương sai đang mất các thuộc tính xác định dương của nó và tôi đoán nó phải làm với những nỗ lực của tôi để cập nhật các tập hợp con của ma trận hiệp phương sai đầy đủ.

Như một ví dụ được lấy từ một tệp nhật ký thực tế, ma trận sau (sau bước dự đoán UKF) là xác định dương:

   1.1969            0            0            0            0            0      0.11567            0            0            0            0            0
        0       1.9682            0            0            0            0            0      0.98395            0            0            0            0
        0            0       1.9682            0            0            0            0            0      0.98395            0            0            0
        0            0            0       1.9682            0            0            0            0            0      0.98395            0            0
        0            0            0            0       1.9682            0            0            0            0            0      0.98395            0
        0            0            0            0            0       1.9682            0            0            0            0            0      0.98395
  0.11567            0            0            0            0            0      0.01468            0            0            0            0            0
        0      0.98395            0            0            0            0            0            1            0            0            0            0
        0            0      0.98395            0            0            0            0            0            1            0            0            0
        0            0            0      0.98395            0            0            0            0            0            1            0            0
        0            0            0            0      0.98395            0            0            0            0            0            1            0
        0            0            0            0            0      0.98395            0            0            0            0            0            1

Tuy nhiên, sau khi xử lý hiệu chỉnh cho một biến (trong trường hợp này là vận tốc X tuyến tính), ma trận trở thành:

   1.1969            0            0            0            0            0      0.11567            0            0            0            0            0
        0       1.9682            0            0            0            0            0      0.98395            0            0            0            0
        0            0       1.9682            0            0            0            0            0      0.98395            0            0            0
        0            0            0       1.9682            0            0            0            0            0      0.98395            0            0
        0            0            0            0       1.9682            0            0            0            0            0      0.98395            0
        0            0            0            0            0       1.9682            0            0            0            0            0      0.98395
  0.11567            0            0            0            0            0         0.01            0            0            0            0            0
        0      0.98395            0            0            0            0            0            1            0            0            0            0
        0            0      0.98395            0            0            0            0            0            1            0            0            0
        0            0            0      0.98395            0            0            0            0            0            1            0            0
        0            0            0            0      0.98395            0            0            0            0            0            1            0
        0            0            0            0            0      0.98395            0            0            0            0            0            1

Sự khác biệt giữa hai ma trận trên là

        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0     -0.00468            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0

Như bạn có thể thấy, sự khác biệt duy nhất giữa hai là giá trị ở vị trí phương sai của vận tốc X tuyến tính, đây là phép đo tôi vừa xử lý. Sự khác biệt này đủ để "phá vỡ" ma trận hiệp phương sai của tôi.

Tôi có hai câu hỏi:

Cập nhật một tập hợp con của bộ lọc dường như không phải là cách phù hợp. Có một giải pháp tốt hơn?
Ngoài ra, tôi có thiếu một bước sẽ giữ ma trận hiệp phương sai của tôi là xác định dương không?

Cảm ơn!

BIÊN TẬP:

Có vẻ như tôi không đặt các giá trị trở lại đúng vào ma trận hiệp phương sai ban đầu. Chỉ cần sao chép lại các giá trị là không đủ. Tôi cần theo dõi các hệ số tương quan cho ma trận hiệp phương sai và đảm bảo rằng khi tôi cập nhật giá trị phương sai, tôi cập nhật tất cả các giá trị trong hàng / cột của nó để duy trì giá trị hệ số tương quan. Tôi phải thực hiện thêm một số thử nghiệm để xác minh rằng đây là vấn đề của tôi, nhưng một số phân tích ban đầu trong Matlab cho thấy rằng đó là vấn đề. Nếu tôi đúng, tôi sẽ trả lời câu hỏi của riêng tôi.

EDIT 2:

Đưa ra phản hồi bên dưới và sau khi thử nó, tôi có thể thấy rằng ý tưởng chỉnh sửa ban đầu của tôi sẽ không bay được. Tuy nhiên, tôi có thêm một câu hỏi:

Vì đây là UKF, tôi thực sự không có ma trận Jacobian. Tôi nghĩ rằng tôi thấy làm thế nào tôi có thể làm cho nó hoạt động trong các phương trình cập nhật ukf, nhưng ngay cả trong một EKF - và tôi hỏi vì tôi có một trong những người là tốt - tôi nhà nước-to-đo chức năng sẽ kết thúc lên được bản sắc ma trận, như tôi đang đo trực tiếp các biến trạng thái của tôi. Trong trường hợp, tôi lấy nó là "Jacobian" của tôi sẽ chỉ là một ma trận với các ma trận ở vị trí , trong đó là chỉ số của các giá trị đo được trong vectơ đo lường? $h$ $m \times n$ $(i, i)$ $i$

kalman-filter

— TheWumpus
nguồn

Một vài lưu ý đầu tiên,

Đầu tiên, như bạn đã đề cập, bạn không thể lấy ra một hàm con và thực hiện cập nhật về nó. Tuy nhiên, bạn có thể thực hiện một bước di chuyển trên một con tàu con.

Điều này là do các điều khoản hiệp phương sai (thông tin "lan truyền" trên các phần khác nhau của tiểu bang). Đây là lý do tại sao có một ước tính chính xác hơn về tiêu đề của bạn sẽ dẫn đến ước tính vị trí chính xác hơn, ví dụ.

Tuy nhiên, sau khi chỉnh sửa, chỉ cần cập nhật các điều khoản tương quan chéo (hiệp phương sai) sẽ không làm điều đó, bạn cần cập nhật toàn bộ ma trận (trừ khi bạn biết chắc chắn rằng một số yếu tố độc lập, dựa trên ước tính trạng thái.

Ở đây bạn đi:

Để làm điều này, hãy tạo các ma trận Jacobian như trước đây, nhưng lưu ý rằng cần có các số 0 trong tất cả các phần tử nằm ngoài đường chéo khi không có phần nào của trạng thái đó được đo. Sau đó, phép thuật đảo ngược ma trận sẽ lan truyền các hiệu chỉnh đổi mới đến các phần chính xác của trạng thái. Ma trận Jacobian phải có kích thước đối với giá trị đo và biến trạng thái (hoặc tùy theo định nghĩa của bạn về Jacobian). Không ai trong số này "cập nhật một phần của rác hiệp phương sai" trừ khi bạn biết chắc chắn rằng các yếu tố Jacobian tương đương với danh tính. Cách an toàn nhất là sử dụng Jacobian đầy đủ. $n\times m$ $m$ $n$ $m\times n$

Các hack khác (một khi mọi thứ đều đúng về mặt lý thuyết)

Tuy nhiên, điều này vẫn không đảm bảo ma trận hiệp phương sai PD. Tôi mạnh mẽ mạnh mẽ khuyên bạn không làm hacks sau cho đến khi bạn khắc phục tất cả những sai lầm khác. Nhưng cuối cùng, đối với một hệ thống có khả năng triển khai tại hiện trường hoạt động với lượng thời gian không hề nhỏ, tôi thấy rằng hầu như luôn luôn phải làm những việc sau:

Sau tất cả các cập nhật, Đặt hiệp phương sai là , chỉ để "chẵn" các thuật ngữ ngoài đường chéo - cho đối xứng $P$ $P\gets \frac{1}{2}P + \frac{1}{2}P^{T}$
Đặt , trong đó là một vô hướng nhỏ để đảm bảo bạn không bị tràn (và phá hủy số điều kiện của ma trận của bạn) $P\gets P + \epsilon I_{n\times n}$ $\epsilon$

— Josh Vander Hook
nguồn