Kỹ thuật lấy mẫu Unscented Transform (UT) tính toán các số liệu thống kê của một biến ngẫu nhiên bằng cách sử dụng phép biến đổi phi tuyến. Nó dựa trên nguyên lý cơ bản sau: việc xấp xỉ phân bố Gauss là dễ thực hiện hơn việc xấp xỉ một hàm phi tuyến hoặc là một phép biến đổi phi tuyến bất kì [34]. Phép biến đổi UT có thểđược tóm tắt như sau [33]:
o Giả sử vector 𝑥𝑥(có kích thước là 𝑛𝑛× 1) có kì vọng là 𝑥𝑥̅ và ma trận hiệp phương sai 𝑃𝑃. Cho một hàm phi tuyến 𝑦𝑦=ℎ�𝑥𝑥�, các giá trị 𝑦𝑦�𝑜𝑜 và các giá trị 𝑃𝑃𝑜𝑜 là các giá trịước lượng cho kỳ vọng và phương sai của 𝑦𝑦.
o Tính 2𝑛𝑛 vector sigma 𝑥𝑥(𝑚𝑚)như sau:
𝑥𝑥(𝑚𝑚) = 𝑥𝑥̅ +𝑥𝑥�(𝑚𝑚) 𝑖𝑖 = 1, … ,2𝑛𝑛
𝑥𝑥�(𝑚𝑚) = �√𝑛𝑛𝑃𝑃�𝑚𝑚𝑇𝑇 𝑖𝑖 = 1, … ,𝑛𝑛 𝑥𝑥�(𝑚𝑚+𝑚𝑚) = −�√𝑛𝑛𝑃𝑃�𝑚𝑚𝑇𝑇 𝑖𝑖 = 1, … ,𝑛𝑛
(4.10)
trong đó √𝑛𝑛𝑃𝑃 là ma trận căn bậc hai của ma trận 𝑛𝑛𝑃𝑃, trong đó
�√𝑛𝑛𝑃𝑃�𝑇𝑇√𝑛𝑛𝑃𝑃=𝑛𝑛𝑃𝑃 và chỉ số𝑖𝑖 là hàng thứ𝑖𝑖 của ma trận.
o Biến đổi các vector signma theo công thức:
𝑦𝑦(𝑚𝑚) = ℎ�𝑥𝑥(𝑚𝑚)� 𝑖𝑖 = 1, … ,2𝑛𝑛 (4.11)
o Ước lượng kì vọng và hiệp phương sai của 𝑦𝑦như sau:
𝑦𝑦𝑜𝑜 = 21𝑛𝑛� 𝑦𝑦(𝑚𝑚) 2𝑚𝑚 𝑚𝑚=1 𝑃𝑃𝑜𝑜 = 21𝑛𝑛� �𝑦𝑦(𝑚𝑚)− 𝑦𝑦𝑜𝑜� �𝑦𝑦(𝑚𝑚)− 𝑦𝑦𝑜𝑜�𝑇𝑇 2𝑚𝑚 𝑚𝑚=1 (4.12)
Hình 4.1 mô tả ví dụ về hệ thống hai chiều khi được áp dụng bộ quan sát EKF và UKF, trong đó:
26
o Hình bên trái ngoài cùng thể hiện kỳ vọng và phương sai khi triển khai
phương pháp trích mẫu Monte – Carlo.
o Hình ở giữa thể hiện kết quả khi áp dụng việc tuyến tính hóa của phương pháp EKF.
o Hình bên phải là trường hợp áp dụng kỹ thuật UT. Có thể thấy chất lượng của bộ kỹ thuật này khá là tốt.
Hình 4.1. So sánh giá trị trung bình và phương sai trong trường hợp áp dụng các bộ quan sát khác nhau [32].
4.2.2 Thuật toán triển khai
Thuật toán triển khai áp dụng bộ quan sát UKF [33] được trình bày như sau:
Bước 1: Tại 𝑘𝑘 = 0, khởi tạo các giá trị:
𝑥𝑥�0+ = 𝐸𝐸�𝑥𝑥0�
𝑃𝑃0+ = 𝐸𝐸 ��𝑥𝑥0− 𝑥𝑥�0+��𝑥𝑥0− 𝑥𝑥�0+�𝑇𝑇� (4.13)
Bước 2: Dựđoán. Bước này được thực hiện giữa hai phép đo liên tiếp từ𝑡𝑡𝑘𝑘−1 đến 𝑡𝑡𝑘𝑘. Khi đó tiến hành ước lượng giá trị trạng thái và ma trận hiệp phương sai theo các bước sau:
• Bước 2.1: Chọn 2𝑛𝑛 vector sigma 𝑥𝑥𝑘𝑘−1(𝑚𝑚) tương tự như trong (4.10) dựa trên trạng thái ước lượng và ma trận hiệp phương sai ở bước trước là
27 𝑥𝑥𝑘𝑘−1(𝑚𝑚) = 𝑥𝑥�𝑘𝑘−1+ +𝑥𝑥�(𝑚𝑚) 𝑖𝑖 = 1, … ,2𝑛𝑛 𝑥𝑥�(𝑚𝑚) = ��𝑛𝑛𝑃𝑃𝑘𝑘−1+ � 𝑚𝑚 𝑇𝑇 𝑖𝑖 = 1, … ,𝑛𝑛 𝑥𝑥�(𝑚𝑚+𝑚𝑚) = − ��𝑛𝑛𝑃𝑃𝑘𝑘−1+ � 𝑚𝑚 𝑇𝑇 𝑖𝑖 = 1, … ,𝑛𝑛 (4.14)
• Bước 2.2: Tìm các trạng thái 𝑥𝑥�(𝑚𝑚) nhờ việc ánh xạ2𝑛𝑛 vector sigma 𝑥𝑥𝑘𝑘−1(𝑚𝑚)
thông qua phương trình phi tuyến sai phân 𝑓𝑓(∘)như sau:
𝑥𝑥�(𝑚𝑚) = 𝑓𝑓�𝑥𝑥𝑘𝑘−1(𝑚𝑚) ,𝑢𝑢𝑘𝑘−1,𝑡𝑡𝑘𝑘� 𝑖𝑖 = 1, … ,2𝑛𝑛 (4.15)
• Bước 2.3: Kết hợp các vector 𝑥𝑥�(𝑚𝑚) này với nhau từ đo thu được trạng thái ước lượng tại thời điểm 𝑡𝑡𝑘𝑘:
𝑥𝑥�𝑘𝑘− =21𝑛𝑛� 𝑥𝑥�(𝑚𝑚)
2𝑚𝑚 𝑚𝑚=1
(4.16)
• Bước 2.4: Ma trận hiệp phương sai được ước lượng qua công thức:
𝑃𝑃𝑘𝑘− =21𝑛𝑛��𝑥𝑥�(𝑚𝑚)− 𝑥𝑥�𝑘𝑘−��𝑥𝑥�(𝑚𝑚)− 𝑥𝑥�𝑘𝑘−�𝑇𝑇 2𝑚𝑚
𝑚𝑚=1
+𝑄𝑄𝑘𝑘−1 (4.17)
Bước 3: Hiệu chỉnh. Bước này được tiến hành khi nhận được giá trị đo. Khi đó giá trịước lượng trạng thái hệ thống và ma trận hiệp phương sai được cập nhật lại theo các bước sau:
• Bước 3.1: Chọn 2𝑛𝑛 vector sigma 𝑥𝑥�𝑘𝑘(𝑚𝑚)tương tựnhư trong (4.10) dựa trên trạng thái ước lượng 𝑥𝑥�𝑘𝑘− và ma trận hiệp phương sai 𝑃𝑃𝑘𝑘−. Cụ thểnhư sau:
𝑥𝑥�𝑘𝑘(𝑚𝑚) = 𝑥𝑥�𝑘𝑘−+𝑥𝑥�(𝑚𝑚) 𝑖𝑖 = 1, … ,2𝑛𝑛
𝑥𝑥�(𝑚𝑚) = ��𝑛𝑛𝑃𝑃𝑘𝑘−�𝑚𝑚𝑇𝑇 𝑖𝑖 = 1, … ,𝑛𝑛 𝑥𝑥�(𝑚𝑚+𝑚𝑚) = −��𝑛𝑛𝑃𝑃𝑘𝑘−�𝑚𝑚𝑇𝑇 𝑖𝑖 = 1, … ,𝑛𝑛
(4.18)
• Bước 3.2: Sử dụng phương trình đo ℎ(∘) để tìm các giá trị 𝑦𝑦�𝑘𝑘(𝑚𝑚) từ các giá trị 𝑥𝑥�𝑘𝑘(𝑚𝑚):
𝑦𝑦�𝑘𝑘(𝑚𝑚) = ℎ�𝑥𝑥�𝑘𝑘(𝑚𝑚),𝑡𝑡𝑘𝑘� 𝑖𝑖 = 1, … ,2𝑛𝑛 (4.19)
• Bước 3.3: Kết hợp các giá trị 𝑦𝑦�𝑘𝑘(𝑚𝑚) để thu được giá trị đo ước lượng tại thời điểm 𝑡𝑡𝑘𝑘:
𝑦𝑦�𝑘𝑘 = 21𝑛𝑛� 𝑦𝑦�𝑘𝑘(𝑚𝑚) 2𝑚𝑚 𝑚𝑚=1
(4.20)
28 𝑃𝑃𝑦𝑦 = 1 2𝑛𝑛� �𝑦𝑦�𝑘𝑘(𝑚𝑚)− 𝑦𝑦�𝑘𝑘� �𝑦𝑦�𝑘𝑘(𝑚𝑚)− 𝑦𝑦�𝑘𝑘�𝑇𝑇 2𝑚𝑚 𝑚𝑚=1 +𝑅𝑅𝑘𝑘 (4.21)
• Bước 3.5: Ước lượng ma trận tương quan giữa trạng thái ước lượng 𝑥𝑥�𝑘𝑘−
và giá trịđo 𝑦𝑦�𝑘𝑘 như sau:
𝑃𝑃𝑚𝑚𝑦𝑦 =21𝑛𝑛��𝑥𝑥�𝑘𝑘(𝑚𝑚)− 𝑥𝑥�𝑘𝑘−� �𝑦𝑦�𝑘𝑘(𝑚𝑚)− 𝑦𝑦�𝑘𝑘�𝑇𝑇 2𝑚𝑚
𝑚𝑚=1
(4.22)
• Bước 3.6: Tiến hành cập nhật trạng thái ước lượng và ma trận hiệp phương sai như sau:
𝐾𝐾𝑘𝑘 = 𝑃𝑃𝑚𝑚𝑦𝑦𝑃𝑃𝑦𝑦−1
𝑥𝑥�𝑘𝑘+ = 𝑥𝑥�𝑘𝑘−+𝐾𝐾𝑘𝑘�𝑦𝑦𝑘𝑘 − 𝑦𝑦�𝑘𝑘� 𝑃𝑃𝑘𝑘+ = 𝑃𝑃𝑘𝑘−− 𝐾𝐾𝑘𝑘𝑃𝑃𝑦𝑦𝐾𝐾𝑘𝑘𝑇𝑇
(4.23)
Bước 4: Tăng 𝑘𝑘 lên một đơn vị và cập nhật lại giá trị cho giá trị ước lượng và ma trận hiệp phương sai:
𝑘𝑘 ≔ 𝑘𝑘 + 1
𝑥𝑥�𝑘𝑘−1+ ≔ 𝑥𝑥�𝑘𝑘+
𝑃𝑃𝑘𝑘−1+ ≔ 𝑃𝑃𝑘𝑘+ (4.24)
Sau đó quay trở lại bước 2.