Guidance, Navigation & Control (GNC)

Level: 5 — Mastery (cross-domain: math + physics + coding, build/prove, open-ended) Time limit: 3 hours Total marks: 100

Instructions: Answer all three questions. Show all derivations. Where code is requested, pseudocode or Python/NumPy is acceptable but must be dimensionally and algorithmically correct. State all assumptions.

---

Question 1 — Attitude Kinematics, Quaternions & Gimbal Lock (34 marks)

A spacecraft has body angular velocity (rad/s) measured in the body frame: $\boldsymbol{\omega} = \begin{bmatrix} 0.10 \\ -0.20 \\ 0.05 \end{bmatrix}.$

(a) Starting from the definition of the direction cosine matrix $C$ (body-from-inertial) and the requirement that a body-fixed vector has constant components, derive the DCM kinematic equation $\dot C = -[\boldsymbol{\omega}\times]C$. Explicitly justify the sign. (6)

(b) Write the quaternion kinematic equation $\dot{\mathbf q} = \tfrac12 \Xi(\mathbf q)\,\boldsymbol\omega$ in full component form for the scalar-first convention $\mathbf q = (q_0,q_1,q_2,q_3)$. Prove that this kinematics preserves the unit-norm constraint $\|\mathbf q\| = 1$ (i.e. $\frac{d}{dt}\|\mathbf q\|^2 = 0$). (8)

(c) For the 3-2-1 (yaw–pitch–roll) Euler sequence, the body-rate to Euler-rate map is $\begin{bmatrix}\dot\phi\\\dot\theta\\\dot\psi\end{bmatrix} = \frac{1}{\cos\theta}\begin{bmatrix} \cos\theta & \sin\phi\sin\theta & \cos\phi\sin\theta \\ 0 & \cos\phi\cos\theta & -\sin\phi\cos\theta \\ 0 & \sin\phi & \cos\phi \end{bmatrix}\boldsymbol\omega .$ Explain quantitatively what happens at $\theta = \pm 90^\circ$ (gimbal lock): identify the singular quantity, show which Euler rates diverge, and state which physical rotational degree of freedom becomes unobservable in Euler-angle space. (6)

(d) Given the current attitude quaternion $\mathbf q(0) = (1,0,0,0)$ and the $\boldsymbol\omega$ above (assumed constant), write a self-contained code routine that propagates $\mathbf q$ for $T = 10\text{ s}$ using RK4 with step $h = 0.01$ s, renormalizing each step, and returns the final quaternion and the equivalent 3-2-1 Euler angles. State the exact analytic final quaternion (rotation about the fixed axis $\hat{\boldsymbol\omega}$) and use it to argue your code is correct. (8)

(e) Modified Rodrigues Parameters (MRP) are $\boldsymbol\sigma = \hat{\mathbf e}\tan(\Phi/4)$ for principal rotation angle $\Phi$. State the value of $\Phi$ at which the standard MRP set becomes singular, and describe the shadow-set switching rule that keeps the representation bounded. (6)

---

Question 2 — INS/GPS Sensor Fusion & Kalman Filtering (34 marks)

Consider a 1-D inertial navigation problem: a vehicle moves along one axis. An accelerometer measures specific force $a_m = a + b + w_a$ where $b$ is a slowly-varying bias (random walk) and $w_a$ is white noise. A GPS receiver provides noisy position fixes.

State vector: $\mathbf x = [p,\ v,\ b]^\top$ (position, velocity, accelerometer bias).

(a) Write the continuous-time INS mechanization / error model in state-space form $\dot{\mathbf x} = A\mathbf x + B u + G\mathbf w$, treating the measured acceleration input as $u = a_m$ and the bias as a random-walk driven by process noise. Give $A$, $B$, $G$ explicitly. (6)

(b) GPS measures position: $z = p + \nu$, $\nu\sim\mathcal N(0,R)$. Write $H$. Test observability of the pair $(A,H)$ using the observability matrix rank test. Is the accelerometer bias $b$ observable from position measurements alone? Prove your answer. (8)

(c) Derive the Kalman gain $K_k$ from first principles as the matrix that minimizes $\operatorname{tr}(P_k^+)$, the trace of the a-posteriori covariance, starting from the standard update $\mathbf x_k^+ = \mathbf x_k^- + K_k(z_k - H\mathbf x_k^-)$ and the Joseph-form covariance. Show $K_k = P_k^- H^\top (HP_k^-H^\top + R)^{-1}$. (10)

(d) A complementary filter blends a high-rate integrated-IMU estimate with low-rate GPS. Write the scalar complementary-filter transfer functions from (i) true signal and (ii) each sensor to the output, and show the two sensor paths sum to unity across all frequencies. Explain in one line why the crossover frequency is chosen near the point where IMU drift power equals GPS noise power. (6)

(e) State the separation principle (LQG) precisely and explain why observability (part b) and controllability are the two conditions that make an LQG controller well-posed. (4)

---

Question 3 — Control Design & Proportional Navigation Guidance (32 marks)

A single-axis rocket pitch model (rigid, thrust-vector-controlled) linearizes to the double-integrator-with-instability plant: $\ddot\theta = k\,\delta,\qquad G(s) = \frac{k}{s^2},$ where $\theta$ is pitch angle, $\delta$ is the TVC gimbal angle command, and $k = 4.0\ \text{s}^{-2}$.

(a) Design a PD controller $\delta = -(K_p\theta + K_d\dot\theta)$ to place the closed-loop poles at $s = -2 \pm 2j$. Solve for $K_p, K_d$. Give the resulting undamped natural frequency $\omega_n$ and damping ratio $\zeta$. (8)

(b) Put the closed-loop system in state-space form with $\mathbf x = [\theta,\dot\theta]^\top$, write $A_{cl}$, compute its eigenvalues, and confirm they match (a). Then propose a Lyapunov function $V = \mathbf x^\top P \mathbf x$ and state the condition on $P$ that proves asymptotic stability. (8)

(c) The real TVC actuator has finite bandwidth modelled as a first-order lag $\frac{\delta_{act}}{\delta_{cmd}}=\frac{1}{\tau s+1}$ with $\tau = 0.05$ s, plus a pure transport delay $e^{-t_d s}$, $t_d = 0.02$ s. For the PD-compensated loop, estimate the phase margin degradation contributed by the lag and delay at the gain-crossover frequency $\omega_c \approx 2.8$ rad/s. Comment on stability-margin risk. (8)

(d) Derive proportional navigation guidance: for a constant-bearing collision geometry, show the commanded lateral acceleration is $a_c = N V_c \dot\lambda$, defining every symbol. Explain the geometric meaning of "driving $\dot\lambda \to 0$" and state the typical range of the navigation constant $N$. Then state the augmentation term added for a maneuvering/gravity target and why. (8)

---

import numpy as np
def Xi(q):
    q0,q1,q2,q3=q
    return 0.5*np.array([[-q1,-q2,-q3],[q0,-q3,q2],
                         [q3,q0,-q1],[-q2,q1,q0]])
def qdot(q,w): return Xi(q)@w
def propagate(q0,w,T=10.0,h=0.01):
    q=np.array(q0,float); n=int(T/h)
    for _ in range(n):
        k1=qdot(q,w); k2=qdot(q+h/2*k1,w)
        k3=qdot(q+h/2*k2,w); k4=qdot(q+h*k3,w)
        q=q+h/6*(k1+2*k2+2*k3+k4); q/=np.linalg.norm(q)
    return q
def quat_to_euler321(q):
    q0,q1,q2,q3=q
    phi=np.arctan2(2*(q0*q1+q2*q3),1-2*(q1**2+q2**2))
    theta=np.arcsin(2*(q0*q2-q3*q1))
    psi=np.arctan2(2*(q0*q3+q1*q2),1-2*(q2**2+q3**2))
    return phi,theta,psi
qf=propagate([1,0,0,0],np.array([0.1,-0.2,0.05]))