VINS系统约束构建推导

qianlima

---

state vector

$$x_k = [x_{b_{k-N+1}},,,x_{b_{k-1}},x_{b_{k}},v^w_{bk},b_{a_k},b_{g_k},s_k]$$
机器人状态和参数，以imu系为主，
为了维持计算量不随着时间增大，维护10帧的滑动窗口。

注意：
1. k-n+1~k-1的N帧为滑动窗口，$s_k$为地面参数，见《manifold constraint》
2. 和VINS不同本文中的imu约束只优化当前帧的v,ba,bg
3. 原文优化odo姿态，本文优化imu姿态，原文中$x_k = [x_{O_{k-N+1}},,,x_{O_{k-1}},x_{O_{k}},v^w_{bk},b_{a_k},b_{g_k},s_k]，x_{O_i}=[p^w_{O_i},q^w_{O_i}]$

cost function

manifold constraint

manifold:
quarduaric manifold: https://www.zybuluo.com/snuffles/note/1727090

vision constraint

projection
projection_pw
epopilar

motion constraint

A.odo factor中只观察vx,vy,wz三个维度:https://www.zybuluo.com/snuffles/note/1726657
B.odo factor中包含slip: https://www.zybuluo.com/snuffles/note/1726575
odo的w以imu值来估计

Tracking

LK optical flow tracking(opencv)
good features to track(Harris)

Initialize

Margin

Margin Old, Margin Second New

Loop detection & optimize