def compute_localization(self, i, previousState, insMeasures,
previousInsMeasure, view):
C, y, Gamma_obs = self.__generate_observations(i, view)
correct_gravity = np.array([0, 0, 9.81])
# =============================================================================
# Angular integration
# =============================================================================
angular_velocity = insMeasures[3:6]
previousAngles = previousState.orientation
previousR = rotmat(previousAngles[0], previousAngles[1],
previousAngles[2])
angles = self.dt * angular_velocity + previousAngles
R = rotmat(angles[0], angles[1], angles[2])
# =============================================================================
# Velocities integration after correction of rotation and gravity
# =============================================================================
velocities = self.dt * (
previousR @ previousInsMeasure[0:3] + R @ insMeasures[0:3] +
2 * correct_gravity) / 2 + previousState.linear_velocity
# =============================================================================
# Kalman filter for computing pose
# =============================================================================
pose, Gamma = kalman(previousState.pose, self.A, previousState.gamma,
self.dt * velocities, self.B, self.gammaINS, y,
Gamma_obs, C)
estimatedState = RobotState(pose,
angles,
velocities,
angular_velocity,
Gamma,
view=view)
return estimatedState
def integrateINS(self, dt, measures, previous_measure, history, view,
gammaINS):
C, y, Gamma_obs = self.generateObservationForKalman(view, "TBN_SURF")
# C = None
# y = None
# Gamma_obs = None
correct_gravity = np.array([0, 0, 9.81])
previous_state = history[-1]
# =============================================================================
# Angular integration
# =============================================================================
angular_velocity = measures[3:6]
angles = dt * angular_velocity + previous_state.orientation
R = rotmat(angles[0], angles[1], angles[2])
# =============================================================================
# Velocities integration after correction of rotation and gravity
# =============================================================================
measures[0:3] = R @ measures[0:3]
velocities = dt * (
previous_measure[0:3] + measures[0:3] +
2 * correct_gravity) / 2 + previous_state.linear_velocity
# =============================================================================
# Kalman filter for computing pose
# =============================================================================
pose, Gamma = kalman(previous_state.pose, self.A, previous_state.gamma,
dt * velocities, self.B, gammaINS, y, Gamma_obs,
C)
return pose, angles, velocities, angular_velocity, Gamma
y2 = -6
Galpha = rl.eye(2)
Gbeta = 1
xhat0 = rl.array([[0], [0]])
Gx0 = 100 * eye(2)
rl.close("all")
rl.ion()
fig = rl.figure("Kalman 3 steps")
ax = fig.add_subplot(1, 1, 1)
rl.draw_ellipse(xhat0, Gx0, 0.9, ax, "black")
# --------------------------------
xhat1, Gx1 = rl.kalman(xhat0, Gx0, u0, y0, Galpha, Gbeta, A0, C0)
rl.draw_ellipse(xhat1, Gx1, 0.9, ax, "red")
xup0, Gup0 = rl.kalman_correc(xhat0, Gx0, y0, Gbeta, C0)
rl.draw_ellipse(xup0, Gup0, 0.9, ax, "black", style="--")
# --------------------------------
xhat2, Gx2 = rl.kalman(xhat1, Gx1, u1, y1, Galpha, Gbeta, A1, C1)
rl.draw_ellipse(xhat2, Gx2, 0.9, ax, "green")
xup1, Gup1 = rl.kalman_correc(xhat1, Gx1, y1, Gbeta, C1)
rl.draw_ellipse(xup1, Gup1, 0.9, ax, "red", style="--")
# --------------------------------
xhat3, Gx3 = rl.kalman(xhat2, Gx2, u2, y2, Galpha, Gbeta, A2, C2)
rl.draw_ellipse(xhat3, Gx3, 0.9, ax, "magenta")
xup2, Gup2 = rl.kalman_correc(xhat2, Gx2, y2, Gbeta, C2)