多狀態約束卡爾曼濾波器的即時更新策略

Core Concepts

本文提出了一種針對多狀態約束卡爾曼濾波器（MSCKF）的即時更新策略，通過在每個相機幀中利用特徵量測來構建觀測約束，從而提高視覺慣性里程計（VIO）的估計精度。

Abstract

文獻回顧

• 多狀態約束卡爾曼濾波器（MSCKF）以其高效性而聞名，但其估計性能，尤其是在長時間任務中，仍有待提升。
• 現有研究主要集中在通過保持VIO系統的內在可觀測性或減輕卡爾曼濾波器的線性化誤差來改善濾波一致性。
• 與基於滑動窗口優化的VIO方法相比，MSCKF的估計精度仍有不足。

本文貢獻

• 提出了一種適用於MSCKF的即時更新策略，並從信息濾波的角度進行了理論分析，指出即時更新本質上可以獲得更準確的線性化點。
• 通過仿真和基於公開數據集的實驗，驗證了即時更新策略的有效性，結果顯示該策略相較於延遲更新策略，在仿真、EuRoC數據集和KAIST VIO數據集上的姿態估計精度分別提高了約28%、29%和30%。
• 本文提出的MSCKF即時更新策略可以很容易地與其他現代濾波框架相結合，例如UKF、IEKF和EqF等。

即時更新策略

• 與延遲更新策略不同，即時更新策略在新相機姿態和特徵觀測可用時，就立即在線重建特徵點的位置，並利用量測約束對窗口內的IMU狀態和相機姿態進行校正。

理論分析

• 即時更新策略構建了比延遲更新策略更多的量測約束，從特徵觀測中積累了更多信息，從而產生更小的協方差矩陣。
• 即時更新策略中狀態校正的執行頻率更高，這使得量測雅可比矩陣和殘差的線性化點更加準確，進一步提高了濾波一致性。

實驗結果

• 在MATLAB中進行的數值仿真結果表明，即時更新策略相較於延遲更新策略，姿態估計精度提高了約20%，位置精度提高了約28%。
• 在EuRoC數據集和KAIST VIO數據集上進行的實驗也表明，即時更新策略優於延遲更新策略，分別提高了29%和30%的估計精度。

結論

• 本文提出的即時更新策略通過更頻繁地利用特徵觀測信息，有效提高了MSCKF的估計精度。
• 儘管即時更新策略帶來了更高的計算負擔，但通過調整每次更新時構建的觀測約束數量，可以在效率和精度之間取得平衡。

Stats

在模擬中，全相機即時更新和三相機即時更新的MSCKF相較於延遲更新的MSCKF，姿態估計精度提高了約20%。
全相機即時更新的MSCKF在位置精度方面比延遲更新的MSCKF高出約28%，而效率更高的三相機即時更新的MSCKF在位置精度方面也高出21%。
在EuRoC數據集中，全相機/三相機更新策略在MSCKF中分別比延遲策略的性能高出29%和18%。
在KAIST VIO數據集中，三相機和全相機即時更新分別將估計精度提高了22%和30%。

Quotes

Key Insights Distilled From

An Immediate Update Strategy of Multi-State Constraint Kalman Filter

by Qingchao Zha... at arxiv.org 11-05-2024

https://arxiv.org/pdf/2411.02028.pdf

Deeper Inquiries

如何在資源受限的平台上有效地實現即時更新策略？

在資源受限的平台上，實現即時更新策略需要在計算效率和估計精度之間取得平衡。以下是一些可以考慮的優化方法：

降低特徵點數量： 可以通過減少前端提取的特徵點數量來降低計算量。例如，可以調整網格設置、特徵點提取的閾值等參數。
選擇性更新： 可以根據特徵點的質量、觀測次數等因素，選擇性地對部分特徵點進行即時更新，而不是對所有特徵點都進行更新。
簡化更新策略： 可以使用3-cam或5-cam等簡化的即時更新策略，而不是使用all-cam更新策略，以減少每次更新的計算量。
優化代碼實現： 可以通過代碼優化，例如使用SIMD指令集、多線程并行等技術，來提高算法的運行效率。
使用更輕量級的濾波器： 可以考慮使用UKF、IEKF等計算量更小的濾波器來代替EKF，以降低算法的整體計算量。
需要注意的是，這些優化方法可能會在一定程度上降低估計精度，因此需要根據具體的應用場景和性能需求進行權衡和選擇。

除了提高估計精度外，即時更新策略對VIO系統的其他性能指標有何影響？

除了提高估計精度外，即時更新策略還會對VIO系統的其他性能指標產生影響，主要體現在以下幾個方面：

計算複雜度： 相比於延遲更新策略，即時更新策略需要更頻繁地進行狀態更新，因此會增加計算複雜度。尤其是在特徵點數量較多的情況下，計算量的增加會更加明顯。
延遲： 由於即時更新策略需要在每個相機幀都進行狀態更新，因此可能會引入額外的延遲。這對於實時性要求較高的應用場景來說是一個需要考慮的因素。
魯棒性： 即時更新策略可以更及時地利用最新的觀測信息來修正狀態估計，因此在一定程度上可以提高系統的魯棒性。例如，在出現運動模糊、特徵點遮擋等情況時，即時更新策略可以更快地恢復狀態估計。
總體而言，即時更新策略在提高估計精度的同時，也會增加計算複雜度和延遲。因此，在實際應用中需要根據具體的應用場景和性能需求來選擇合適的更新策略。

如何將即時更新策略推廣到其他類型的传感器融合應用中？

即時更新策略的核心思想是及時利用最新的觀測信息來修正狀態估計，以提高估計精度和系統響應速度。這種思想可以推廣到其他類型的传感器融合應用中，例如：

激光雷達-慣性導航系統（LIDAR-IMU）： 可以將即時更新策略應用於處理激光雷達點雲數據，例如在提取到新的特徵點後立即進行狀態更新，以提高系統的定位和建圖精度。
GPS-慣性導航系統（GPS-IMU）： 可以將即時更新策略應用於處理GPS觀測數據，例如在接收到新的GPS信號後立即進行狀態更新，以提高系統的定位精度和抗遮擋能力。
多传感器融合定位系統： 可以將即時更新策略應用於融合多種傳感器數據，例如將相機、激光雷達、IMU、GPS等傳感器數據進行融合，并在接收到新的觀測數據後立即進行狀態更新，以提高系統的整體性能。
在推廣即時更新策略時，需要根據具體的應用場景和傳感器特性對算法進行適當的調整和優化。例如，需要考慮不同傳感器的數據頻率、噪聲特性、觀測模型等因素，以設計合理的狀態更新策略和濾波器參數。