AE 410/641 - Navigation and Guidance

Quiz-1 Solution
Sep 4th, 2023

Q1) Derive q̇ = 12 q ẇ from the first principle

Sol: Using first principles:

q(t + ∆t) − q(t)
q̇ = lim (1)
∆t→0 ∆t

q(t + ∆t) is the current attitude obtained by post-multiplying the previous attitude q(t) with ∆qL
when time moves forward by ∆t, since the change is defined locally.

q(t) ◦ ∆qL − q(t)

∴ q̇ = lim (2)
∆t→0 ∆t

∆qL is defined as follows:

cos ∆θ2 L
 
∆qL =
~nsin ∆θ2 L

Using small angle approximation, cos ∆θ2 L ≈ 1 and sin ∆θ2 L ≈ ∆θL
2
. Therefore, ∆qL can now
be written as follows:  
1
∆qL = ∆θL (3)
~n 2

Substituting (3) in (2):

   
1 1
q(t) ◦ [ −
~n ∆θ2 L 0
∴ q̇ = lim (4)
∆t→0 ∆t
 
0
q ◦ ∆θL
~n 2
q̇ = lim (5)
∆t→0 ∆t
 
1 0
q̇ = q ◦ (6)
2 lim∆t→0 ~n ∆θ L
∆t

1
∴ q̇ = q ◦ w (7)
2

Hence derived.
 Q2) Consider an attitude determination problem of spacecraft using the TRIAD method. The
observations are following:
The direction of Sun in the body frame: SB = [0.8138, −0.5438, 0.2049]T
The direction of earth magnetic field in the body frame: MB = [−0.3420, −0.1632, 0.9254]T
The direction of Sun in the inertial frame : SI = [0.8138, −0.5438, 0.2049]T
The direction of earth magnetic field in the inertial frame : MI = [−0.3420, −0.1632, 0.9254]T
Determine the rotation matrix from the above observations.
Sol: By TRIAD Method,
Considering SB and SI to have more accurate components than the magnetic field vector. We
consider the accurate vector to be our first vector of the triad.

T1B = SB , T1I = SI
SB × MB SI × MI
T2B = , T2I =
|SB × MB | |SI × MI |
T1B × MB T1B × MB
= , =
|T1B × MB | |T1B × MB |
T3B = T2B × T1B , T3I = T2I × T1I = T3B
[T1B , T2B , T3B ] = R[T1I , T2I , T3I ]
Taking inverse on both sides,

 
1 0 0
R = 0 1 0
0 0 1

Q3) If there are multiple observations in the above question. How can we formulate the prob-
lem? Define the loss function to determine the optimal solution and give the steps to solve
the optimal attitude determination problem using quaternion.

Sol: Suppose, the transformation is represented as

b = Ar (8)

In the case of multiple observations, we can minimize the following cost function
X
J = (b − Ar)T (b − Ar) (9)
= bT b + (Ar)T (Ar) − 2bT Ar (10)
= 2(1 − bT Ar) (11)

After doing some algebraic manipulations, we can obtain the following maximization problem:

J¯ = tr(ArbT ) (12)
= tr(AB T ) (13)

2
Alternatively, we can use the QUEST algorithm to formulate the above problem for multiple
observations, wherein we are provided with n non-parallel reference vectors (in the body frame)
V̂i and n corresponding observation vectors (in the inertial frame) Ŵi . A set of non-negative
weights ai exists such that ni=1 ai = 1. For simplicity, we can assume a1 = a2 = ..... = an =
P
1/n.
The loss function is defined as follows:
n
1X
L(A) = |Ŵi − AV̂i |2
2 i=1

A is the orthogonal matrix that minimizes the loss function. The gain function g(A) is defined by:
n
X
g(A) = 1 − L(A) = ai ŴiT AV̂i
i=1

The loss function L(A) is at its minimum when the gain function is at its maximum. The gain
function can be reformulated as:
n
X
g(A) = ai tr(ŴiT AV̂i ) = tr(AB T )
i=1

where tr is the trace of a matrix, and B is the attitude profile matrix. In order to find the rotation
matrix, the following formula is used:
n
X
B= ai ŴiT AV̂i
i=1

The quaternion q is defined as follows:

   
q0 X̂sin 2θ
q= =
Q cos 2θ

Now the gain function can be rewritten in terms of quaternions:

g(q) = (q02 − Q.Q)trB T + 2tr(QQT B T ) + 2q0 (tr[Q]x B T )

Here [Q]x is the antisymmetric matrix of Q:
 
0 Q3 −Q2
[Q]x = −Q3 0 Q1 
Q2 −Q1 0

A further simplification gives:

g(q) = q T Kq

3
where the 4x4 matrix K is given by:
 
S − σI Z
K=
ZT σ

Using the helper values:

σ = trB
S = B + BT
Xn
Z= ai (Ŵi × V̂i )
i=1

A new gain function g’(q) with Lagrange multipliers is defined:

g(q) = q T Kq − λq T q

It is verified that Kq = λq. Therefore, the optimal attitude qopt , which maximises g(q), is found
to be the eigenvector of K belonging to the largest eigenvalue of K:

Kqopt = λqopt

Q4) What is dead reckoning? Explain it using example.

Sol: Dead reckoning is a navigation technique used to estimate the current position of a vehicle or
object based on a previously known position, initial velocity, heading(direction) and time elapsed.
It is essentially a method of estimating one’s position by continuously updating it based on the
direction and speed of movement. For example, the position P (t) at time t can be estimated by
integrating the velocity V (t) over time :
Z t
P (t) = P0 + V (τ ) dτ
0

The error in dead reckoning accumulates over time due to inaccuracies in velocity and heading
measurements. The longer the time interval between updates and the less accurate the mea-
surements, the larger the error becomes. To mitigate this error, dead reckoning is often used in
conjunction with other navigation techniques, such as GPS or external sensors, to periodically
correct and update the estimated position.

Q5) Explain the static calibration method for accelerometer and show how the 12 parameters
(scale factors and biases) can be obtained from the gravity.

Sol: The static calibration method involves mounting the inertial system on a leveled surface with
sensitive x, y, z axes of the IMU pointing alternately up and down. The bias and scale factor errors
are calculated using,
lfup + lfdown
b=
2
4
 lfup − lfdown − 2 × K
S=
2×K
Where, lfup is the sensor measurement when the sensitive axis is pointed upward and lfdown is the
measurement when the sensitive axis is pointed downwards and K is the known reference signal.
      
lax mxx mxy mxz ax bax
lay  = myx myy myz  ay  + bay 
laz mzx mzy mzz az baz

Here the diagonal m elements represent the scale factors, the off diagonal elements are the non
orthogonalities and the b components are the biases. By aligning the IMU using the standard
6-position method, the ideal accelerations would be measured as:
           
g −g 0 0 0 0
a1 = 0 , a2 = 0 , a3 = g , a4 = −g , a5 = 0 , a6 = 0 
          
0 0 0 0 g −g
Consequently, the design matrix(A) for the least squares adjustment will have the form:
 
a1 a2 a3 a4 a5 a6
A=
1 1 1 1 1 1

The raw output of the sensors constitutes the matrix U:

 
U = u1 u2 u3 u4 u5 u6

We can extract the components of the matrix M using the least squares method as,

M = U AT (AAT )−1