1.
A UAV has its magnetometer readout is (20𝑥⃗, 20𝑦⃗, −40𝑧⃗) mT and its accelerometer readout
is (0.2𝑥⃗, −0.3𝑦⃗, −0.932𝑧⃗)𝐺. Please calculate the North, East, and Down vectors in the body
fixed frame. (No need to calculate the attitude of the drone)
Solution:
The down direction can be calculated directly from the accelerometer. Therefore we obtain:
1111⃗! = (0.2𝑥⃗, −0.3𝑦⃗, −0.932𝑧⃗)
Here we denote 𝐴
1111⃗
𝐴! (0.2𝑥⃗, −0.3𝑦⃗, −0.932𝑧⃗))
11111111111111⃗! =
𝐷𝑜𝑤𝑛 = = (0.2𝑥⃗, −0.3𝑦⃗, −0.932𝑧⃗)
1111⃗! 7 |(0.2𝑥⃗, −0.3𝑦⃗, −0.932𝑧⃗)|
7𝐴
The East direction can be obtained from the cross product of the magnetometer readout (which
shows the strength and the direction of magnetic fields) and the accelerometer readout (or Down
1111⃗! = (20𝑥⃗, 20𝑦⃗, −40𝑧⃗):
vector). Here we denote 𝐵
1111⃗! × 𝐷𝑜𝑤𝑛
𝐵 11111111111111⃗! (−30.64𝑥⃗, 10.64𝑦⃗, −10𝑧⃗)
11111111111⃗
𝐸𝑎𝑠𝑡! = = = (−0.903𝑥⃗, 0.313𝑦⃗, −0.295𝑧⃗)
11111111111111⃗! 7 |(−30.64𝑥⃗, 10.64𝑦⃗, −10𝑧⃗)|
1111⃗! × 𝐷𝑜𝑤𝑛
7𝐵
Finally, the North direction can be obtained from the cross product of the East direction and the
Down direction (accelerometer readout).
11111111111⃗
𝐸𝑎𝑠𝑡! × 𝐷𝑜𝑤𝑛 11111111111111⃗! (−12.916𝑥⃗, −30.556𝑦⃗, 7.064𝑧⃗)
111111111111111⃗! =
𝑁𝑜𝑟𝑡ℎ =
11111111111111⃗! 7 |(−12.916𝑥⃗, −30.556𝑦⃗, 7.064𝑧⃗)|
11111111111⃗! × 𝐷𝑜𝑤𝑛
7𝐸𝑎𝑠𝑡
111111111111111⃗! = (−0.381𝑥⃗, −0.901𝑦⃗, 0.208𝑧⃗)
𝑁𝑜𝑟𝑡ℎ
2. Based on Question 1, If there is an error from a hard iron source of (−5𝑥⃗, −5𝑦⃗, 0𝑧⃗) (mT in
the above readout, what is the error in the attitude measurement?
Solution:
The accelerometer is not affected:
11111111111111⃗" = 𝐷𝑜𝑤𝑛
𝐷𝑜𝑤𝑛 11111111111111⃗! = (0.2𝑥⃗, −0.3𝑦⃗, −0.932𝑧⃗)
The corrected magnetometer readout:
1111⃗
𝐵" = (20𝑥⃗, 20𝑦⃗, −40𝑧⃗) − (−5𝑥⃗, −5𝑦⃗, 0𝑧⃗) =(25𝑥⃗, 25𝑦⃗, −40𝑧⃗)
The corrected East direction:
1111⃗" × 111111111111⃗
𝐵 𝐷𝑜𝑤𝑛" (−35.3𝑥⃗, 15.3𝑦⃗, −12.5𝑧⃗)
11111111111⃗
𝐸𝑎𝑠𝑡" = = = (−0.873𝑥⃗, 0.378𝑦⃗, −0.309𝑧⃗)
𝐷𝑜𝑤𝑛" 7 |(−35.3𝑥⃗, 15.3𝑦⃗, −12.5𝑧⃗)|
1111⃗" × 11111111111111⃗
7𝐵
The corrected North direction:
11111111111⃗
𝐸𝑎𝑠𝑡" × 𝐷𝑜𝑤𝑛 11111111111111⃗" (−18𝑥⃗, −35.4𝑦⃗, 7.53𝑧⃗)
111111111111111⃗" =
𝑁𝑜𝑟𝑡ℎ = = (−0.446𝑥⃗, −0.876𝑦⃗, 0.186𝑧⃗)
11111111111111⃗" 7 |(−18𝑥⃗, −35.4𝑦⃗, 7.53𝑧⃗)|
11111111111⃗" × 𝐷𝑜𝑤𝑛
7𝐸𝑎𝑠𝑡
Since the iron has no effect on the accelerometer:
111111111111⃗ = 0
Δ𝐷𝑜𝑤𝑛
The error on East and North directions are:
111111111⃗ = 11111111111⃗
Δ𝐸𝑎𝑠𝑡 𝐸𝑎𝑠𝑡! − 11111111111⃗
𝐸𝑎𝑠𝑡" = (−0.903𝑥⃗, 0.313𝑦⃗, −0.295𝑧⃗) − (−0.873𝑥⃗, 0.378𝑦⃗, −0.309𝑧⃗)
111111111⃗
Δ𝐸𝑎𝑠𝑡 = (−0.03𝑥⃗, −0.065𝑦⃗, 0.014𝑧⃗)
111111111111⃗ = 111111111111111⃗
Δ𝑁𝑜𝑟𝑡ℎ 𝑁𝑜𝑟𝑡ℎ! − 111111111111111⃗
𝑁𝑜𝑟𝑡ℎ" = (−0.381𝑥⃗, −0.901𝑦⃗, 0.208𝑧⃗) − (−0.446𝑥⃗, −0.876𝑦⃗, 0.186𝑧⃗)
111111111111⃗
Δ𝑁𝑜𝑟𝑡ℎ = (0.065𝑥⃗, −0.025𝑦⃗, 0.022𝑧⃗)
3. After removing the error produced from Question2, we introduce (0.05𝑥⃗, −0.1𝑦⃗, 0.1𝑧⃗) 𝐺
linear acceleration by controlling the UAV, how much error will it cause in the attitude
measurement?
Solution:
The correct magnetometer readout:
1111⃗# = 1111⃗
𝐵 𝐵" =(25𝑥⃗, 25𝑦⃗, −40𝑧⃗)
The new accelerometer readout:
1111⃗# = (0.2𝑥⃗, −0.3𝑦⃗, −0.932𝑧⃗)𝐺 + (0.05𝑥⃗, −0.1𝑦⃗, 0.1𝑧⃗) 𝐺 = (0.25𝑥⃗, −0.4𝑦⃗, −0.832𝑧⃗) 𝐺
𝐴
The new NED vectors:
1111⃗#
𝐴
11111111111111⃗# =
𝐷𝑜𝑤𝑛 = (0.261𝑥⃗, −0.418𝑦⃗, −0.870𝑧⃗)𝐺
1111⃗# 7
7𝐴
1111⃗# × 𝐷𝑜𝑤𝑛
𝐵 11111111111111⃗#
11111111111⃗# =
𝐸𝑎𝑠𝑡 = (−0.883𝑥⃗, 0.259𝑦⃗, −0.390𝑧⃗)
1111⃗# × 𝐷𝑜𝑤𝑛
7𝐵 11111111111111⃗# 7
11111111111⃗
𝐸𝑎𝑠𝑡# × 𝐷𝑜𝑤𝑛 11111111111111⃗#
111111111111111⃗# =
𝑁𝑜𝑟𝑡ℎ = (−0.389𝑥⃗, −0.871𝑦⃗, 0.302𝑧⃗)
11111111111⃗# × 𝐷𝑜𝑤𝑛
7𝐸𝑎𝑠𝑡 11111111111111⃗# 7
So the error :
111111111111⃗ = 𝐷𝑜𝑤𝑛
Δ𝐷𝑜𝑤𝑛 11111111111111⃗# − 11111111111111⃗
𝐷𝑜𝑤𝑛" = (0.061𝑥⃗, −0.118𝑦⃗, 0.062𝑧⃗)
111111111⃗ = 11111111111⃗
Δ𝐸𝑎𝑠𝑡 𝐸𝑎𝑠𝑡# − 11111111111⃗
𝐸𝑎𝑠𝑡" = (−0.010𝑥⃗, −0.119𝑦⃗, −0.699𝑧⃗)
111111111111⃗ = 111111111111111⃗
Δ𝑁𝑜𝑟𝑡ℎ 𝑁𝑜𝑟𝑡ℎ# − 𝑁𝑜𝑟𝑡ℎ 111111111111111⃗" = (0.057𝑥⃗, 0.005𝑦⃗, 0.116𝑧⃗)
4. An Interferometer Fiber Optical Gyroscope has an optical path of 𝐿 = 1 𝑘𝑚, coil diameter =
𝐷 = 15𝑐𝑚, laser light frequency is 𝑓 = 3 × 10!$ Hz, if the input rate is Ω = 0.1°/𝑠, what is
the Sagnac phase shift?
Solution:
𝐿 = 1 𝑘𝑚 = 1000 𝑚
D = 15𝑐𝑚 = 0.15 𝑚
&
𝑐 = 3.0 × 10% 𝑚/𝑠, 𝑓 = 3 × 10!$ 𝐻𝑧, 𝜆 = ' = 1.0 × 10() 𝑚
𝜋
Ω = 0.1°/𝑠 = 0.1 × 𝑟𝑎𝑑/𝑠 = 0.00175 𝑟𝑎𝑑/𝑠
180
2𝜋𝐿𝐷Ω
ΔΦ = = 0.0055 𝑟𝑎𝑑
𝜆𝑐
