Bộ quan sát EKF được sử dụng trong các tài liệu [27], [26] được xem xét cho hệ thống có động học liên tục nhưng lại có phép đo rời rạc như sau:
𝑥𝑥̇(𝑡𝑡) = 𝑓𝑓 �𝑥𝑥(𝑡𝑡),𝑢𝑢(𝑡𝑡),𝜔𝜔(𝑡𝑡)� 𝑦𝑦𝑘𝑘 = ℎ𝑘𝑘�𝑥𝑥𝑘𝑘,𝑣𝑣𝑘𝑘�
𝜔𝜔(𝑡𝑡) ~ (0,𝑄𝑄)
𝑣𝑣𝑘𝑘 ~ (0,𝑅𝑅𝑘𝑘)
(4.1)
Nhiễu quá trình 𝜔𝜔(𝑡𝑡)được giả thiết là nhiễu ổn trắng với phương sai 𝑄𝑄, nhiễu đo 𝑣𝑣𝑘𝑘 cũng được giả thiết là nhiễu ổn trắng rời rạc với phương sai là 𝑅𝑅𝑘𝑘. Giữa các lần đo, giá trị trạng thái ước lượng được tính toán dựa vào mô hình động học hệ thống bởi phương trình đầu tiên trong (4.1). Còn ma trận hiệp phương sai thì thu được từ việc giải phương trình vi phân sau:
𝑃𝑃̇ =𝐴𝐴𝑃𝑃+𝑃𝑃𝐴𝐴+𝐿𝐿𝑄𝑄𝐿𝐿𝑇𝑇 − 𝑃𝑃𝐷𝐷𝑇𝑇(𝑀𝑀𝑅𝑅𝑀𝑀𝑇𝑇)−1𝐷𝐷𝑃𝑃 (4.2) trong đó các ma trận 𝐴𝐴, 𝐿𝐿, 𝐷𝐷, 𝑀𝑀 là các ma trận Jacobian: 𝐴𝐴 = 𝜕𝜕𝑓𝑓𝜕𝜕𝑥𝑥� 𝑚𝑚=𝑚𝑚� 𝐿𝐿 = 𝜕𝜕𝜔𝜔𝜕𝜕𝑓𝑓� 𝑚𝑚=𝑚𝑚� 𝐷𝐷 = 𝜕𝜕ℎ𝜕𝜕𝑥𝑥𝑘𝑘� 𝑚𝑚=𝑚𝑚� 𝑀𝑀 = 𝜕𝜕ℎ𝜕𝜕𝜔𝜔𝑘𝑘� 𝑚𝑚=𝑚𝑚� (4.3)
24 Chú ý rằng trong (4.2), không có thành phần 𝑅𝑅 đi kèm với 𝑃𝑃̇ bởi vì 𝑃𝑃 được tính toán trong giữa thời gian giữa các lần đo liên tiếp nhau, đó là lúc mà các phép đo không được thực hiện. Nói cách khác, giữa các thời điểm đo này là các phép đo với phương sai vô cùng lớn (𝑅𝑅 =∞), như vậy thành phần gần cuối trong (4.2) sẽ tiến dần về0. Như vậy, công thức để tiến hành ước lượng hai giá trị 𝑥𝑥� và 𝑃𝑃 là:
𝑑𝑑𝑥𝑥�
𝑑𝑑𝑡𝑡 = 𝑓𝑓�𝑥𝑥�,𝑢𝑢,𝜔𝜔0,𝑡𝑡� 𝑃𝑃̇ = 𝐴𝐴𝑃𝑃+𝑃𝑃𝐴𝐴𝑇𝑇 +𝐿𝐿𝑄𝑄𝐿𝐿𝑇𝑇
(4.4) Phương trình (4.4) tính 𝑥𝑥� từ giá trịđo ước lượng 𝑥𝑥�𝑘𝑘−1+ và ma trận hiệp phương sai 𝑃𝑃𝑘𝑘−1+ tại thời điểm đo 𝑡𝑡𝑘𝑘−1 thành 𝑥𝑥�𝑘𝑘− và 𝑃𝑃𝑘𝑘− tại thời điểm 𝑡𝑡𝑘𝑘. Chú ý rằng 𝜔𝜔0 là nhiễu danh định trong (4.1), và ởđây được giả thiết là: 𝜔𝜔0(𝑡𝑡) = 0.
Tại thời điểm đo, trạng thái ước lượng và ma trận hiệp phương sai được cập nhật dựa trên công thức của bộ lọc Kalman rời rạc như sau:
𝐾𝐾𝑘𝑘 = 𝑃𝑃𝑘𝑘−𝐷𝐷𝑘𝑘𝑇𝑇(𝐷𝐷𝑘𝑘𝑃𝑃𝑘𝑘−𝐷𝐷𝑘𝑘𝑇𝑇 +𝑀𝑀𝑘𝑘𝑅𝑅𝑘𝑘𝑀𝑀𝑘𝑘𝑇𝑇)−1 𝑥𝑥�𝑘𝑘+ = 𝑥𝑥�𝑘𝑘−+𝐾𝐾𝑘𝑘�𝑦𝑦𝑘𝑘 − ℎ𝑘𝑘�𝑥𝑥�𝑘𝑘−,𝑣𝑣0,𝑡𝑡𝑘𝑘��
𝑃𝑃𝑘𝑘+ = (𝐼𝐼 − 𝐾𝐾𝑘𝑘𝐷𝐷𝑘𝑘)𝑃𝑃𝑘𝑘−(𝐼𝐼 − 𝐾𝐾𝑘𝑘𝐷𝐷𝑘𝑘)𝑇𝑇 +𝐾𝐾𝑘𝑘𝑀𝑀𝑘𝑘𝑅𝑅𝑘𝑘𝑀𝑀𝑘𝑘𝑇𝑇𝐾𝐾𝑘𝑘𝑇𝑇
(4.5) ở đây 𝑣𝑣0 là nhiễu định danh, ởđây sẽđược gán 𝑣𝑣0 = 0. Các ma trận 𝐷𝐷𝑘𝑘 và 𝑀𝑀𝑘𝑘 là đạo hàm riêng của ℎ𝑘𝑘�𝑥𝑥𝑘𝑘,𝑣𝑣𝑘𝑘� lần lượt theo 𝑥𝑥 và 𝑢𝑢 theo (4.3). Cả hai ma trận này đều được tính toán tại 𝑥𝑥 =𝑥𝑥�𝑘𝑘−.