Only Truth Matters
0%

Pre-Integration

Posted on Edited on In , , , ,

1. What

Integrate the measurements from the sensor (e.g. IMU)

2. Why

Obtain a predicted pose and velocity, used to correct the odometry

Advantage:

  • more accurate and robust estimation
  • Enable real-time operation
  • Can handle asynchronous measurements from different sensor

3. How

  1. Collect IMU data
  2. Pre-integrate IMU data
  3. Use Kalman filter to update the state estimation
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
import numpy as np

class Preintegration:
def __init__(self, gravity):
self.gravity = gravity
self.p_bias = np.zeros((3, 1))
self.v_bias = np.zeros((3, 1))
self.R = np.eye(3)
self.p = np.zeros((3, 1))
self.v = np.zeros((3, 1))

def integrate(self, omega, a, dt):
omega = omega - self.bias[:, np.newaxis]
a = a - self.a_bias[:, np.newaxis]

R_old = self.R
p_old = self.p
v_old = self.v

# update orientation
dR = self._skew(omega) @ R_old
self.R = R_old + dR * dt
self.R = self._orthonormalize(self.R)

# update position and velocity
d_v = self.gravity + self.R @ a
self.v = v_old + d_v * dt
self.p = p_old + v_old * dt + 0.5 * d_v * dt * dt

# update covariance
F = np.block([
[np.zeros((3, 3)), -self._skew(a), -self.R],
[np.zeros((3, 3)), np.zeros((3, 3)), self._skew(omega)],
[np.zeros((3, 3)), np.zeros((3, 3)), np.zeros((3, 3))]
])

Q = np.diag([0.01, 0.01, 0.01, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001])**2
self.cov = F @ self.cov @ F.T + Q * dt

def _skew(self, v):
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])

def _orthonormalize(self, R):
u, _, vh = np.linalg.svd(R)
return u @ vh