Research Article
CKF-Based Visual Inertial Odometry for Long-Term Trajectory Operations
| (1) | Initialize the filter state and covariance matrix; | | (2) | for do | | (3) | Use IMU data to predict the nominal filter state with 4th order Runge-Kutta numerical integration; | | (4) | Calculate and | | (5) | Compute the propagated state covariance ; | | (6) | if New Image then | | (7) | Perform feature detection for I3 and match these features with features {m1} and {m2} to have feature tracking ; | | (8) | Factorize: ; | | (9) | while ((Condition (17) is satisfied) and) do | | (10) | Calculate the variation rate using (14); | | (11) | Generate Cubature points using (8); | | (12) | Compute innovation covariance matrix and the predicted measurement: (9) and (10); | | (13) | Compute the new filter state and covariance matrix: (12) and (13); | | (14) | end | | (15) | State transition and rearrange the covariance matrix with respective camara poses; | | (16) | end | | (17) | end |
|
