doublet function

doublet function

system dynamics for propagation model: xdot = f(x, u) x = [phi, theta].T u = [p, q, r, Va].T

system dynamics for propagation model: xdot = f(x, u) x = [pn, pe, Vg, chi, wn, we, psi].T u = [p, q, r,

measurement model y=h(x,u) for accelerometers x = [phi, theta].T u = [p, q, r, Va].T

measurement model for gps measurements: y=y(x, u) x = [pn, pe, Vg, chi, wn, we, psi].T u = [p, q, r, Va,

measurement model measurement model for wind triangale pseudo measurement: y=y(x, u) x = [pn, pe, Vg, chi, wn, we, psi].T

vector to skew symmetric matrix associated with cross product

vector to skew symmetric matrix associated with cross product

impulse function

impulse function

Measurement update update the state xhat and covarance P when a measurement is received Parameters:

wrap x to be between 0 and 2*pi

model propagation - solve xhatdot=f(xhat,u), and Pdot = AP+PA' + Q + G Qu G' between measurements Parameters:

random function

random function

converts a 3-2-1 R_b^i (**b2i = transpose of the book!**) rotation matrix to euler angles

converts a 3-2-1 R_b^i (**b2i = transpose of the book!**) rotation matrix to a unit quaternion

returns rotation matrix for right handed passive rotation about z-axis