def getSigPred(X, w_mean, wrot, dt):
Y = np.zeros(X.shape)
for i, xi in enumerate(X.transpose()):
qt = expq(np.hstack((0, wrot * dt / 2)))
qi = multq(xi, qt)
Y[:, i] = qi
xmean, Evi = avgq(Y.transpose(), w_mean, x)
Pm = 2 * np.dot(Evi[:, 0], Evi[:, 0].transpose()) + np.dot(
Evi[:, 1:], Evi[:, 1:].transpose()) / (2 * n)
return Y, xmean, Evi, Pm
def getSigmaPoints(P, Q, x, n):
X = np.zeros([x.shape[0], 2 * n + 1])
S = np.linalg.cholesky(P + Q)
W = np.sqrt(n) * S
W = np.hstack((W, -W))
X[:, 0] = x
for i, w in enumerate(W.transpose()):
qW = expq(np.hstack((0, w / 2)))
Xi = multq(x, qW)
X[:, i + 1] = Xi
return X
def updateX(Z, zacc, nu, Evi, Pm, R):
Pzz = 2 * np.dot((Z - zacc)[:, 0], (Z - zacc)[:, 0].transpose()) + np.dot(
(Z - zacc)[:, 1:], (Z - zacc)[:, 1:].transpose()) / (2 * n)
Pvv = Pzz + R
Pxz = 2 * np.dot(Evi[:, 0], (Z - zacc)[:, 0].transpose()) + np.dot(
Evi[:, 1:], (Z - zacc)[:, 1:].transpose()) / (2 * n)
K = np.dot(Pxz, np.linalg.inv(Pvv))
P = Pm - np.dot(np.dot(K, Pvv), K.transpose())
V = np.hstack((0, np.dot(K, np.reshape(nu, [
3,
])) / 2))
x = multq(xmean, expq(V))
return K, P, x
w = data[3:][:]
w = np.vstack((w[1, :], w[2, :], w[0, :]))
sen_a = 300
sen_w = 3.33 * 180 / np.pi
sf_a = 3300 / (1023 * sen_a)
sf_w = 3300 / (1023 * sen_w)
bias_a = np.mean(acc[:, :250], 1)
bias_w = np.mean(w[:, :250], 1)
acc = (acc - np.reshape(bias_a, [3, 1])) * sf_a + np.reshape(
np.array([0, 0, 1]), [3, 1])
wrot = (w - np.reshape(bias_w, [3, 1])) * sf_w
q = np.array([1, 0, 0, 0])
qt = q
td = np.diff(ts)
eul = q[1:]
for t in range(0, np.shape(wrot)[1] - 1):
qt = multq(qt, expq(np.hstack((0, wrot[:, t] / 2 * td[0, t]))))
qt = qt / np.linalg.norm(qt)
q = np.vstack((q, qt))
eq = np.reshape(quat2euler(qt), [
3,
])
eul = np.vstack((eul, eq))