def mode_solve_set_pos(self, ra, dec, roll, t, camera): ci = self.cameras[camera] pos_orig = Quaternion([ra, dec, roll]) self.campos_adjust[ci] = (pos_orig, t) if self.t0 is None: self.t0 = t ha = (t - self.t0) / 240.0 qha = self.prec_q * Quaternion([-ha, 0, 0]) / self.prec_q #print "qha", quat_axis_to_ra_dec(qha), "prec", self.prec_ra, self.prec_dec #print Quaternion([ra, dec, roll]).to_euler(), (qha * Quaternion([ra, dec, roll])).to_euler() pos = qha * pos_orig self.pos[ci].append((pos, t)) if ci > 0 and len(self.pos[0]) > 0: prev_pos, prev_t = self.campos_adjust[0] if abs(t - prev_t) < 10: self.campos[ci].append(pos_orig.inv() * prev_pos) elif ci == 0: for i in range(1, len(self.pos)): if len(self.pos[i]) > 0: prev_pos, prev_t = self.campos_adjust[i] if abs(t - prev_t) < 10: self.campos[i].append(prev_pos.inv() * pos_orig)
q_tmp = q_list[j] * xt_bar_q.inv()
vec_tmp = qu.quat2vec(q_tmp)
W_prime[0:3, j] = [vec_tmp[0], vec_tmp[1], vec_tmp[2]]
W_prime[3:6, j] = d_omg[:, j]
# Step 6
Pt_bar = np.zeros((6, 6))
for j in range(2 * n):
Pt_bar += W_prime[:, j].reshape(6, 1) * W_prime[:, j].reshape(1, 6)
Pt_bar /= (2 * n)
# Step 7 Observation -> Accelerometer measurements
#Find Z
Z = np.zeros((6, 2 * n))
for j in range(2 * n):
v = Y[0:4, j]
q_tmp = Quaternion(v[0], v[1], v[2], v[3])
qz = q_tmp.inv() * g * q_tmp
qz.normalize()
vz = qu.quat2vec(qz)
Z[0:3, j] = [vz[0], vz[1], vz[2]]
Z[3:6, j] = Y[4:7, j]
#step 8 Find the mean of prediction Z
zt_bar = np.mean(Z, axis=1)
#find innovation
zt = imu_val[:, i]
vt = zt - zt_bar
# Step 9 & 10 innovation covariance P_vv, P_xz
P_zz = np.zeros((6, 6))
P_xz = np.zeros((6, 6))
diff_z = Z - np.tile(zt_bar.reshape((6, 1)), (1, 12))
for j in range(2 * n):
P_zz += diff_z[:, j].reshape((6, 1)) * diff_z[:, j].reshape((1, 6))
