def acc_model(states):
# maybe should be a unit quat
# maybe +ve g
g = np.array([[0, 0, 0, 9.8]])
inv_states = copy.deepcopy(states[:, :4])
inv_states[:, 1:4] = -inv_states[:, 1:4]
g_body = quat.multiply(quat.multiply(inv_states, g), states[:, :4])
# g_body = quat.multiply(quat.multiply(inv_states, g), states[:, :4])
return g_body[:, 1:4]
def old_acc_model(state):
# maybe should be a unit quat
# maybe +ve g
g = np.array([0, 0, 0, 9.8])
quat_inv = copy.deepcopy(state[:4])
quat_inv[1:4] = -quat_inv[1:4]
g_body = quat.multiply(
quat.multiply(state[np.newaxis, :4], g[np.newaxis, :]),
quat_inv[np.newaxis, :])
return g_body.flatten()[1:]
def quaternion_mean(self):
iter = 0
while (1):
iter += 1
Y_qmean_inv = np.hstack((self.Y_qmean[0], -self.Y_qmean[1:]))
self.ev_i = quat.quat2vec(
quat.multiply(self.Y[:, :4], Y_qmean_inv[np.newaxis, :]))
self.e_quat = quat.init_omega(
np.mean(self.ev_i, axis=0)[np.newaxis, :])
self.Y_qmean = quat.multiply(
self.e_quat, self.Y_qmean[np.newaxis, :]).squeeze()
if np.linalg.norm(np.mean(self.ev_i,
axis=0)) < 0.001 or iter > 10000:
break
self.Y_qmean = self.Y_qmean / np.linalg.norm(self.Y_qmean)
def kalman_update(sigma_pts, measurement):
v = measurement - sigma_pts.Z_mean
kv = np.matmul(sigma_pts.K, v)
q = quat.multiply(sigma_pts.Y_qmean[np.newaxis, :],
quat.init_omega(kv[np.newaxis, :3], 1))
w = sigma_pts.Y_mean[4:] + kv[3:]
return np.hstack((q.flatten(), w))
def find_points(self, state, dt):
# Clean the old points
self.init_ts()
W = np.vstack((np.sqrt(2.5 * self.n) * np.linalg.cholesky(
(self.P + self.Q)).transpose(),
-np.sqrt(2.5 * self.n) * (np.linalg.cholesky(
(self.P + self.Q)).transpose())))
self.X[:4, :] = quat.multiply(state[np.newaxis, :4],
quat.init_omega(W[:, :3])).transpose()
self.X[4:, :] = (state[4:] + W[:, 3:]).T
self.X = self.X.T
self.Y = process_model(self.X, dt)
# for i in self.X:
# self.Z.append(old_sensor_model(i))
self.Z = sensor_model(self.X)
def process_model(states, dt):
q = quat.multiply(states[:, :4], quat.init_omega(states[:, 4:], dt))
w = states[:, 4:]
return np.column_stack((q, w))