def qekf_measurement_model(T, y, n, w):
"""Generate a weighted least squares measurement model.
This is eq. 18 from [2].
Args:
T 3x3 rotation matrix describing the prior attitude
y 3xm matrix of unit 3D vector observations
n 3xm matrix of corresponding unit 3D reference vectors
w list of weights corresponding to y and n
Returns:
A 3x3 measurement Jacobian.
"""
H = np.zeros((3, 3))
for ii in range(y.shape[1]):
# Measurement Jacobian: eq 18
yx = pq.skew(y[0:3, ii])
Tn = T.dot(n[0:3, ii])
Tnx = pq.skew(Tn)
H += w[ii] * (yx.dot(Tnx) + Tnx.dot(yx))
return H
def qekf_measurement_covariance(T, y, n, w, sigma_y, sigma_n):
"""Generate a measurement covariance for the z vector by computing
measurement covariances on the observations and reference vectors.
The z vector is defined as the weighted sum of the cross-products
of the observations and references. If
B = \sum_{i=1}^n w_i y_i n_i^\top
then
\left[ z \times \right] = B^\top - B
or alternatively,
z = \sum_{i=1}^n w_i ( y_i \times n_i ) .
This function first generates QUEST measurement model covariances
for n and y (Eqs. 5(a) and 5(b) from [2]) and then uses these to
produce a measurement covariance for z (Eq. 25 from [2]).
The weights can be obtained using compute_weights(), or you can
supply your own.
Args:
T transformation matrix describing the prior attitude
y 3xm matrix of unit vector observations
n 3xm matrix of corresponding unit reference vectors
a array of measurement weights corresponding to y and n
sigma_y array of standard deviations for each unit vector observation
sigma_n array of standard deviations for each unit reference vector
Returns:
A weighted 3x3 measurement covariance.
"""
R = np.zeros((3, 3))
for ii in range(y.shape[1]):
# QUEST measurement model: eq 5a, 5b
Rnn = quest_measurement_covariance(n[:, ii], sigma_n[ii])
Ryy = quest_measurement_covariance(y[:, ii], sigma_y[ii])
yx = pq.skew(y[0:3, ii])
Tn = T.dot(n[0:3, ii])
Tnx = pq.skew(Tn)
# Measurement covariance: eq 25
yxT = yx.dot(T)
R += (yxT.dot(Rnn.dot(yxT.T)) + Tnx.dot(Ryy.dot(Tnx.T))) * (4 *
w[ii]**2)
return R
def F(self):
x = self.x
T_body_to_inrtl = np.identity(3)
F = np.zeros((self.N, self.N))
F[9:self.N, 9:self.N] = np.diag(-self.beta)
F[0:3, 3:6] = np.identity(3)
F[3:6, 0:3] = G(x.eci[0:3], x.mu_earth) + G(x.lci[0:3], x.mu_moon)
F[3:6, 6:9] = -pq.skew(x.a_meas_inrtl).dot(T_body_to_inrtl)
F[3:6, 9:12] = -T_body_to_inrtl
F[6:9, 6:9] = -pq.skew(x.w_meas_inrtl)
F[6:9, 12:15] = -np.identity(3)
return F
def horizon_update(self, x, P, rel, plot=False):
if rel in ('earth', 399):
body_id = 399
r_pci = x.eci[0:3]
elif rel in ('moon', 301):
body_id = 301
r_pci = x.lci[0:3]
R_cam = horizon.covariance(x.time,
body_id,
fpa_size=x.params.horizon_fpa_size,
fov=x.params.horizon_fov,
theta_max=x.params.horizon_theta_max,
sigma_pix=x.params.horizon_sigma_pix,
n_max=x.params.horizon_n_max)
if plot:
from plot_lincov import plot_covariance
import matplotlib.pyplot as plt
T_lvlh = frames.compute_T_inrtl_to_lvlh(x.lci)[0:3, 0:3]
plot_covariance(T_lvlh.dot(R).dot(T_lvlh.T),
xlabel='downtrack (m)',
ylabel='crosstrack (m)',
zlabel='radial (m)')
plt.show()
H = np.zeros((3, self.N))
H[0:3, 0:3] = x.T_body_to_cam.dot(x.T_inrtl_to_body)
H[0:3,
6:9] = x.T_body_to_cam.dot(pq.skew(x.T_inrtl_to_body.dot(r_pci)))
return H, R_cam
def noisify_line_of_sight_vector(u, sigma):
"""Add misalignment to a line-of-sight unit vector measurement.
Args:
u unit vector measurement (length 3)
sigma standard deviation, either scalar or vector (length 3)
Returns:
A noisy unit vector (length 3).
"""
# First rotate the u vector to be along z
z = np.array([0.0, 0.0, 1.0])
# get axis of rotation
v = np.cross(u, z)
v /= spl.norm(v)
# Get angle about that axis
theta = np.arccos(u.dot(z))
T_z_to_u = pq.Quat.from_angle_axis(theta, *v).to_matrix()
# Now produce a misalignment matrix
e = npr.rand(3) * sigma
T_misalign = np.identity(3) - pq.skew(e)
u_noisy = T_z_to_u.dot(T_misalign.dot(z))
return u_noisy / spl.norm(u_noisy) # normalize it
def dynamics_matrix(self, omega):
"""Linearize the dynamics at the current time point, using the current
IMU measurements and the state.
Args:
omega angular velocity measurement (see propagate())
Returns:
A square matrix of the same size as the state vector.
"""
F = np.zeros((6, 6))
F[0:3, 0:3] = -pq.skew(omega)
F[0:3, 3:6] = -np.identity(3)
F[3:6, 3:6] = -np.identity(3) / self.tau
return F
## Scratch remove axis of rotation from quaternion
u = np.array([[0, 0, 1.]]).T
# u = np.random.random((3,1))
u /= norm(u)
psi_hist, yaw_hist, s_hist, v_hist, qz_hist, qx_hist, qy_hist = [], [], [], [], [], [], []
for i in tqdm(range(10000)):
# qm0 = Quaternion.from_euler(0.0, 10.0, 2*np.pi*np.random.random() - np.pi)
# qm0 = Quaternion.from_euler(0.0, np.pi*np.random.random() - np.pi/2.0, 0.0)
qm0 = Quaternion.from_euler(2*np.pi*np.random.random() - np.pi, np.pi * np.random.random() - np.pi / 2.0, 0.0)
# qm0 = Quaternion.from_euler(270.0 * np.pi / 180.0, 85.0 * np.pi / 180.0, 90.0 * np.pi / 180.0)
# qm0 = Quaternion.random()
w = qm0.rot(u)
th = u.T.dot(w)
ve = skew(u).dot(w)
qp0 = Quaternion.exp(th * ve)
epsilon = np.eye(3) * e
t = u.T.dot(qm0.rot(u))
v = skew(u).dot(qm0.rot(u))
tv0 = u.T.dot(qm0.rot(u)) * (skew(u).dot(qm0.rot(u)))
a_dtvdq = -v.dot(u.T.dot(qm0.R.T).dot(skew(u))) - t*skew(u).dot(qm0.R.T.dot(skew(u)))
d_dtvdq = np.zeros_like(a_dtvdq)
nd = norm(t * v)
d0 = t * v
qd0 = Quaternion.exp(d0)
sk_tv = skew(t * v)
if __name__ == '__main__':
import pyquat.random as pqr
import numpy.random as npr
q_inrtl_to_body = pq.Quat(1.0, -2.0, 3.0, 4.0).normalized()
print("q_ib = {}".format(q_inrtl_to_body))
ref_misalign = npr.randn(3) * 1e-6
sun_obs_misalign = npr.randn(3) * 1e-5
mag_obs_misalign = npr.randn(3) * 1e-5
T_ref_err = np.identity(3) #- pq.skew(ref_misalign)
T_sun_obs_err = np.identity(3) - pq.skew(sun_obs_misalign)
T_mag_obs_err = np.identity(3) - pq.skew(mag_obs_misalign)
mag_truth = np.array([0.0, 0.1, 1.0])
mag_truth /= spl.norm(mag_truth)
sun_truth = np.array([0.5, 0.5, 0.02])
sun_truth /= spl.norm(sun_truth)
Tib = q_inrtl_to_body.to_matrix()
mag_ref = T_ref_err.dot(mag_truth)
mag_ref /= spl.norm(mag_ref)
sun_ref = T_ref_err.dot(sun_truth)
sun_ref /= spl.norm(sun_ref)