Personal use of this material is permitted.

Permission from the author(s) and/or copyright holder(s), must be obtained for all other uses. Please contact us and provide details if
you believe this document breaches copyrights.

Quaternion-based Unscented Kalman Filter for

6-DoF Vision-based Inertial Navigation in
GPS-denied Regions
Khashayar Ghanizadegan and Hashim A. Hashim

Abstract—This paper investigates the orientation, position, navigation between outdoor and indoor environments. Ad-
and linear velocity estimation problem of a rigid-body moving vancing accuracy and reliability of the estimation is critical
in three-dimensional (3D) space with six degrees-of-freedom (6 for enriching user experiences in wayfinding applications, as
DoF). The highly nonlinear navigation kinematics are formulated
to ensure global representation of the navigation problem. A com- documented in recent studies [3]–[7]. In aerospace, the precise
putationally efficient Quaternion-based Navigation Unscented estimation of satellite orientation is pivotal for the functional
Kalman Filter (QNUKF) is proposed on S3 ×R3 ×R3 imitating the integrity of satellites, particularly for the accurate processing
true nonlinear navigation kinematics and utilize onboard Visual- of observational data. This aspect is fundamental to ensuring
Inertial Navigation (VIN) units to achieve successful GPS-denied the reliability and effectiveness of satellite operations [8]–[10].
navigation. The proposed QNUKF is designed in discrete form
to operate based on the data fusion of photographs garnered Autonomous and unmanned robots employ navigation tech-
by a vision unit (stereo or monocular camera) and information niques to enhance the precision of Global Navigation Satellite
collected by a low-cost inertial measurement unit (IMU). The System (GNSS)-based localization systems, such as the Global
photographs are processed to extract feature points in 3D space, Positioning System (GPS) and GLONASS. These enhance-
while the 6-axis IMU supplies angular velocity and accelerom- ments are critical for ensuring operational functionality in
eter measurements expressed with respect to the body-frame.
Robustness and effectiveness of the proposed QNUKF have been environments where GPS signals are obstructed or unavailable
confirmed through experiments on a real-world dataset collected [5], [11]. This capability is particularly vital for Unmanned
by a drone navigating in 3D and consisting of stereo images Aerial Vehicles (UAVs) navigating indoors or within densely
and 6-axis IMU measurements. Also, the proposed approach is constructed urban areas, where direct line-of-sight to satellites
validated against standard state-of-the-art filtering techniques. is frequently obscured. Navigation methods that do not rely
Index Terms—Localization, Navigation, Unmanned Aerial Ve- on GNSS hold significant importance in maritime contexts,
hicle, Sensor-fusion, Inertial Measurement Unit, Vision Unit. where GNSS signals can be unreliable or entirely absent,
especially in deep waters or in the vicinity of harbors [12].
For video of navigation experiment visit: link Such GNSS-independent navigation techniques are crucial for
For video of navigation experiment comparison visit: link ensuring the safety and efficiency of marine operations, where
traditional satellite-based positioning systems may not provide
the necessary accuracy or reliability.

I. I NTRODUCTION
B. Related Work
A. Motivation
A naive approach to localizing an agent in a GNSS-denied
N AVIGATION is generally defined as the process of
determining the position, orientation, and linear velocity
of a rigid-body in space, performed by either an observer
environment is to apply Dead Reckoning (DR) using a 6-
axis Inertial Measurement Unit (IMU) rigidly attached to the
vehicle and comprised of an accelerometer (supplying the ap-
or external entities. Navigation, both in its complete and
parent acceleration measurements) and a gyroscope (supplying
partial forms, plays a crucial role in numerous systems and
angular velocity measurements) [1], [5]. DR is able to produce
fields, significantly improving operational performance and
the vehicle navigation state utilizing gyroscope measurements
efficiency. Its applications range from robotics to smartphones,
and integration of acceleration provided that initial navigation
aerospace, and marines [1], [2], highlighting its vital impor-
state is available [5], [13]. DR is widely utilized for pedestrian
tance in driving technological and scientific progress. In the
position and heading estimation in scenarios where GNSS
smartphone industry, the precise estimation of a pedestrian’s
is unreliable (e.g., indoor environments). Typically, the step
position and heading through their mobile device is essential
count, step length, and heading angle are each determined
for improving location-based services and facilitating seamless
by an independent sensor, and these measurements are then
This work was supported in part by National Sciences and Engineering incorporated into the last known position to estimate the new
Research Council of Canada (NSERC), under the grants RGPIN-2022-04937 position. This method serves as a preprocessing step for the
and DGECR-2022-00103. DR, and aims to enhance the accuracy of heading estimation.
K. Ghanizadegan and H. A. Hashim are with the Department of Mechanical
and Aerospace Engineering, Carleton University, Ottawa, Ontario, K1S-5B6, However, a major limitation of DR is the accumulation of
Canada (e-mail: [email protected]). errors in the absence of any measurement model, leading
K. Ghanizadegan and H. A. Hashim, ”Quaternion-based Unscented Kalman Filter for 6-DoF Vision-based Inertial Navigation in GPS-denied Regions,”
IEEE Transactions on Instrumentation and Measurement, 2025. doi: 10.1109/TIM.2024.3509582
 2

to drift over time. In the pedestrian DR domain, multiple monocular-based systems, they encounter difficulties in dark
solutions have been proposed to mitigate the drift issue, such environments and in absence of clear features, such as a drone
as collection of distance information from known landmarks, facing a blank wall. Fusing data from IMUs and vision sensors
a deep-learning based approach to supply filtered acceleration is a widely accepted solution to address navigation challenges,
data [6], Recurrent Neural Network (RNN) with adaptive as these modalities complement each other [5], [27]–[29]. This
estimation of noise covariance [14] and others. These ap- integration of vision sensors and IMU is commonly referred to
proaches are subsequently coupled with a Kalman-type filter as Visual-Inertial Navigation (VIN) [30]. Kalman-type filters
to propagate the state estimate. However, these methods, while have been extensively used to address the VIN problem
enhancing the robustness of the filter, fail to correct the state due to their low computational requirements and ability to
estimate if significant drift occurs. handle non-linear models via linearization. An early significant
In scenarios where a robot navigates in a known envi- contribution in this area was made by Mourikis et al. [29], who
ronment, leveraging measurements from the environment can developed a multi-state Kalman filter-based algorithm for a
be a solution to the drift problem encountered by the pure VIN system that operates based on the error state dynamics.
IMU-based navigation algorithms. Ultra-wideband (UWB) has This approach allows the orientation, which has three degrees
recently gained popularity as an additional sensor in the of freedom, to be represented by a three-dimensional vector.
navigation suite. A robot equipped with a UWB tag gains To address the issue of computational complexity and expand
knowledge of its positions by communicating with fixed UWB the algorithm proposed in [29], the work in [31] explored
anchors with known positions [2], [15]–[18]. This is beneficial various methods for fusing IMU and vision data using the
for structured environments with sufficient fixed reference Extended Kalman Filter (EKF). Specifically, they examined
points (e.g., ship docking [17] and warehouse [15]). However, the integration of these data sources during the EKF prediction
the need for infrastructure with known fixed anchors limits and update phases. Although EKF is an efficient algorithm,
the generalizability of UWB-based navigation algorithms in its major shortcoming is the use of local linearization which
unstructured regions [3]. With advances in three-dimensional disregards the high nonlinearity of the navigation kinematics.
(3D) point cloud registration and processing technologies, Therefore, there is a need for multiple interacting models to
such as the Iterative Closest Point (ICP) [19] and Coherent tackle EKF shortcomings [32]. Moreover, EKF is designed to
Point Drift (CPD) [20], the dependency on initial environment account only for white noise attached to the measurements,
knowledge is significantly reduced. Utilizing these algorithms, making it unfit for measurements with colored noise [8].
a robot equipped only with its perspective of 3D points at mul- Contributions: Unscented Kalman Filter (UKF) generally
tiple steps can reconstruct the environment. This reconstructed outperforms the EKF [8] since it can accurately capture the
environment then is treated as known, allowing the feature nonlinear kinematics while handling white and colored noise
points obtained from the environment to serve as independent [8], [33]. Motivated by the above discussion, the contributions
measurements alongside the IMU [1], [21]. Technologies such of this work are as follows: (1) Novel geometric Quaternion-
as Sound Navigation And Ranging (SONAR), Light Detection based Navigation Unscented Kalman Filter (QNUKF) is pro-
And Ranging (LIDAR), and visual inputs from mono or posed on S3 × R3 × R3 in discrete form mimicking the true
stereo cameras exemplify such measurements. SONAR and nonlinear navigation kinematics of a rigid-body moving in the
LIDAR sensors emit mechanical and electromagnetic waves, 6 DoF; (2) The proposed QNUKF is tailored to supply attitude,
respectively, and construct 3D point clouds by measuring the position, and linear velocity estimates of a rigid-body equipped
distances that each wave travels. SONAR is a popular sensor with a VIN unit and applicable for GPS-denied navigation; (3)
in marine applications [22], as water propagates mechanical The computational efficiency and robustness of the proposed
waves (sound waves) significantly better than electromagnetic QNUKF is tested using a real-world dataset collected by a
waves (light waves). LIDAR is commonly used in space quadrotor flying in the 6 DoF at a low sampling rate and
application [23] since sound waves cannot propagate through subsequently compared to state-of-the-art industry standard
space while light can. However, both LIDAR and SONAR filtering techniques.
struggle in complex indoor and urban environments due to
their inability to capture colour and texture. Therefore, vision- D. Structure
based aided navigation (stereo or monocular camera + IMU)
The remainder of the paper is organized as follows: Section
has emerged as a reliable navigation alternative for GPS-
II discusses preliminary concepts and mathematical founda-
denied regions. In particular, modern UAVs are equipped with
tions; Section III formulates the nonlinear navigation kinemat-
high-resolution cameras which are more cost-effective and
ics problem; Section IV introduces the structure of the novel
computationally reliable.
QNUKF; Section V validates the algorithm’s performance
using a real-world dataset; and Section VI provides concluding
C. Vision-based Navigation and Contributions remarks.
Feature coordinates in 3D space can be extracted using
a monocular camera (given a sequence of two images with II. P RELIMINARIES
persistent vehicle movement) or a stereo (binocular) camera Notation: In this paper, the set of integers, positive inte-
[5], [24]. While stereo vision-based navigation algorithms, gers, and m1 -by-m2 matrices of real numbers are represented
such as those proposed in [25] and [26], generally outperform by Z, Z+ , and Rm1 ×m2 , respectively. The Euclidean norm of
 3

TABLE I: Nomenclature subject to singularities [35]. Angle-axis and Rodrigues vector

{B} / {W} : Fixed body-frame / fixed world-frame parameterization are also subject to singularity in multiple
SO (3) : Special Orthogonal Group of order 3 configurations [35]. Unit-quaternion provides singularity-free
S3 : Three-unit-sphere
orientation representation, while being intuitive and having
Z, Z+ : Integer and positive integer space
only 4 parameters with 1 constraint to satisfy the 3 DoF [35].
qk , q̂k : True and estimated quaternion at step k
A quaternion vector q is defined in the scaler-first format by
pk , p̂k : True and estimated position at step k q = [qw , qx , qy , qz ]⊤ = [qw , qv⊤ ]⊤ ∈ S3 with qv ∈ R3 , qw ∈ R,
vk , v̂k : True and estimated linear velocity at step k and
S3 := q ∈ R4 ||q|| = 1

re,k , pe,k , ve,k : Attitude, position, and velocity estimation error (5)
ak , am,k : True and measured acceleration at step k Let ⊗ denote quaternion product of two quaternion vectors.
ωk , ωm,k : True and measured angular velocity at step k The quaternion product of two rotations q1 = [qw1 , qv1 ]⊤ and
nω,k , na,k : Angular velocity and acceleration measurements q2 = [qw2 , qv2 ]⊤ is given by [35]:
noise
bω,k , ba,k : Angular velocity and acceleration measurements q3 = q1 ⊗ q2
bias  ⊤

qw1 qw2 − qv1 qv2
C× : Covariance matrix of n× . = ∈ S3 (6)
qw1 qv2 + qw2 qv1 + [qv1 ]× qv2
f b , fw : Feature points coordinates in {B} and {W}.
xk , xa k , uk : The state, augmented state, and input vectors at The inverse quaternion of q = [qw , qx , qy , qz ]⊤ = [qw , qv⊤ ]⊤ ∈
the kth time step S3 is represented by
ẑk , zk : Predicted and true measurement
a },
{Xi|j }, {Xi|j : Sigma points of state, augmented state, and q −1 = [qw , −qx , −qy , −qz ]⊤ = [qw , −qv⊤ ]⊤ ∈ S3 (7)
{Zi|j } measurements
Note that qI = [1, 0, 0, 0]⊤ refers to the quaternion identity
where q ⊗ q −1 = qI . The rotation matrix can be described
√ using quaternion Rq (q) such that Rq : S3 → SO(3) [35]:
a vector v ∈ Rmv is defined by ||v|| = v ⊤ v, where v ⊤ refers
to the transpose of v. Im is identity matrix with dimension m- 2
Rq (q) = (qw − ||qv ||2 )I3 + 2qv qv⊤ + 2qw [qv ]× ∈ SO(3) (8)
by-m. The world-frame {W} refers to a reference-frame fixed
Also, quaternion can be extracted given a rotation matrix R ∈
to the Earth and {B} describes the body-frame which is fixed
SO(3) and the mapping qr : SO(3) → S3 is defined by [35]:
to the vehicle’s body. Table I provides important notation used   1p
throughout the paper. The skew-symmetric [·]× of v ∈ R3 is 1 + R(1,1) + R(2,2) + R(3,3)
 
qw 2
1
defined by: 4qw (R(3,2) − R(2,3) )
 qx   
qr (R) =  qy  = 
  1

    (R (1,3) − R(3,1) )
−v3 v2

0 v1 4qw
1
[v]× =  v3 0 −v1  ∈ so(3), v =  v2  (1) qz 4qw (R(2,1) − R(1,2) )
−v2 v1 0 v3 (9)
The rigid-body’s orientation can be extracted using angle-axis
The operator vex(·) is the inverse mapping skew-symmetric parameterization through a unity vector u = [u1 , u2 , u3 ] ∈ S2
operator to vector vex : so(3) → R3 rotating by an angle θ ∈ R [35] where S2 := { u ∈ R3 ||u|| =
vex([v]× ) = v ∈ R3 (2) 1}. The rotation vector r can be defined using the angle-axis
parameterization such that rθ,u : R × S2 → R3 :
The anti-symmetric projection operator Pa (·) : R3×3 → so(3)
r = rθ,u (θ, u) = θu ∈ R3 (10)
is defined as:
1 The rotation matrix can be extracted given the rotation vector
Pa (M ) = (M − M ⊤ ) ∈ so(3), ∀M ∈ Rm×m (3) and the related map is given by Rr : R3 → SO(3) [35]
2
Rr (r) = exp([r]× ) ∈ SO(3)
A. Orientation Representation 2
= I3 + sin(θ) [u]× + (1 − cos(θ)) [u]× (11)
SO(3) denotes the Special Orthogonal Group of order 3 and
is defined by: The unity vector and rotation angle can be obtained from
rotation matrix θR , uR : SO(3) → R3 × R [35]:
SO(3) := R ∈ R3×3 det(R) = +1, RR⊤ = I3

(4)   
Tr(R) − 1
θ = arccos ∈R

where R ∈ SO(3) denotes orientation of a rigid-body. Euler  R

2
angle parameterization provides an intuitive representation of (12)
1 2
vex(Pa (R)) ∈ S

the rigid-body’s orientation in 3D space, often considered uR =

sin θR
analogous to roll, pitch, and yaw angles around the x, y, and
z axes, respectively. This representation has been adopted in Given the rotation vector r in (10), in view of (11) and (9),
many pose estimation and navigation problems such as [22], one has [35]
[34]. However, this representation is kinematically singular qr (r) = qR (Rr (r)) ∈ S3
and not globally defined. Thus, Euler angles fail to repre- ⊤
= cos(θ/2), sin(θ/2)u⊤ ∈ S3

sent the vehicle orientation in several configurations and are (13)
 4

Finally, to find the rotation vector rq (q) corresponding to the An m-dimensional RV V ∈ Rm drawn from a Gaussian
quaternion q, equations (8), (12), and (10) are used as follows: distribution with a mean of V ∈ Rm and a covariance matrix
of Pv ∈ Rm×m is represented by the following:
rq (q) = rθ,u (θR (Rq (q)), uR (Rq (q))) ∈ R3 (14)
V ∼ N (V , Pv )
B. Summation, Deduction Operators, and Weighted Average The Gaussian probability density function of V is formulated
Quaternions and rotation vectors cannot be directly added or below:
subtracted in a straightforward manner, whether separately or P(V ) = N (V |V , Pv )
side-dependent. Let us define the side-dependent summation ⊕
exp − 12 (V − V )⊤ Pv−1 (V − V )


and subtraction ⊖ operators to enable quaternion and rotation = p ∈R
vector summation. In view of (2), (7), (10), and (13), one has: (2π)m det(Pv )
Let:
q ⊕ r := qr (r) ⊗ q ∈ S3 (15)  
A
  
A PA PA,B

∼N , (19)
q ⊖ r := qr (r)−1 ⊗ q ∈ S3 (16) B B ⊤
PA,B PB
where q ∈ S3 denotes quaternion vector and r ∈ R3 denotes Then, given (19), the conditional probability function P(A|B)
rotation vector. The equations (15) and (16) provide the can be calculated as:
resultant quaternion after subsequent rotations represented by
P(A|B) = N (A, B|A + PA,B PB−1 (B − B),
q combined with r, and q combined with the inverse of r,
respectively. Using (6), (7), and (14), the subtraction operator PA − PA,B PB−1 PA,B
⊤
) (20)
of q1 ∈ S3 and q2 ∈ S3 maps S3 × S3 → R3 and is defined
as follows: D. The Unscented Transform
Consider the square symmetric semi-positive definite matrix
q1 ⊖ q2 := rq (q1 ⊗ q2−1 ) ∈ R3 (17)
M ∈ RmM ×mM . Using Singular Value Decomposition (SVD),
The expression (17) provides the rotation vector that represents let U , S, and V ∈ RmM ×mM be the matrices of left
the orientation error between the two quaternions q1 and q2 . singular vectors, singular values, and right singular vectors,
Note that the expressions in (15), (16), and (17) do not only that M = U SV ⊤ . The matrix square root
respectively, such √
resolve the addition and subtraction operations in the S3 and of M , denoted as M , is given by [37]:
R3 spaces, but also provide physical meaning for these op- √ √
M = U SV ⊤ ∈ RmM ×mM , (21)
erations, specifically in terms of subsequent orientations. The
expression (17) provides the rotation vector that represents the where the square root of a diagonal matrix, such as S, is com-
orientation error between the two quaternions q1 and q2 . The puted by taking the square root of its diagonal elements. The
algorithms provided in [8], [36] are used to obtain weighted Unscented Transform (UT) is an approach used to estimate the
mean WM(Q, W ) of a set of quaternions Q = {qi ∈ S3 } and probability distribution of a RV after it undergoes through a
scaler weights W = {wi }. In order to calculate this weighted nonlinear transformation [38]. The problem addressed by the
average, first the 4-by-4 matrix M is found by: UT involves determining the resultant distribution of a random
X variable D ∈ RmD after it is connected through a nonlinear
M= wi qi qi⊤ ∈ R4×4 function fnl (.) to another random variable C ∈ RmC , where
Next, the unit eigenvector corresponding to the eigenvalue with the distribution of C is known:
the highest magnitude is regarded as the weighted average. In D = fnl (C)
other words:
Let C be from a Gaussian distribution with a mean of C and a
WM(Q, W ) = EigVector(M )i ∈ S3 (18) covariance matrix of PC . The sigma points representing P(C),
denoted as the set {Cj } are calculated as:
where
C0 = C ∈ RmC

i = argmax(|EigValue(M )i |) ∈ R 

 p 
Cj = C +

with EigVector(M )i in (18) and EigValue(M )i being the ith (mC + λ)PC
eigenvector and eigenvalue of M , respectively. 
 p j
Cj+m = C −

(m C + λ)P C , j = {1, 2, . . . , 2mC }
C
j
C. Probability p (22)

where λ ∈ R is a scaling parameter, and (mC + λ)PC
The probability of a Random Variable (RV) A being equal j
to a, where a ∈ RmA , is denoted by P(A = a), or more is the jthe column of matrix square root of (mC + λ)PC (as
compactly, P(a). Consider another RV B with a possible value defined in (21)) of (mC + λ)PC . Hence, from (22) each sigma
b ∈ RmB . The conditional probability of A = a given that point Cj will be propagated through the nonlinear function and
B = b, denoted by P(a|b), can be expressed as: sigma points representing P(D), denoted as the set {Dj } are
calculated as:
P(a, b)
P(a|b) = Dj = fnl (Cj ) j = {0, 2, . . . , 2mC }
P(b)
 5

Accordingly, the weighted mean and covariance of the set Let qk ∈ S3 , pk ∈ R3 , vk ∈ R3 , ωk ∈ R3 , and ak ∈ R3
{Dj } provide an accurate representation up to the third degree represent q ∈ S3 , p ∈ R3 , v ∈ R3 , ω ∈ R3 , and a ∈ R3 at
of the real mean and covariance matrix of D. The weights can the kth discrete time step, respectively. The system kinematics
be found as: can then be discretized with a sample time of dT , in a similar

w m = λ manner to the approach described in [1], [21]:
 0 ∈R
λ + mC
    


 qk qk−1
 λ  pk   pk−1 
w0c = + 1 − α2 + β ∈ R c
 vk  = exp(Mk−1 dT )  vk−1  (28)
   

 λ + mC
1

 1 1
wjm = wjc = ∈ R, j = {1, 2, . . . , 2mC }


2(mC + λ) c
(23) where Mk−1 = M c (qk−1 , ωk−1 , ak−1 ). At the kth time
where α and β are scaling parameters in R. Given (23), the step, the measured angular velocity ωm,k ∈ R3 and linear
estimated mean D ˆ and covariance P̂ of the distribution are acceleration am,k ∈ R3 are affected by additive white noise
D
calculated as follows: and biases (bω,k ∈ R3 for angular velocity and ba,k ∈ R3
for linear acceleration), which are modeled as random walks.
2mC
ˆ = X wm D The relationship between the true and measured values can be
D j j (24)
expressed as:
j=0

2m
XC ωm,k = ωk + bω,k + nω,k
ˆ ˆ ⊤

mD ×mD
wjc [Dj − D][Dj − D] ∈ R

P̂D = (25) 
a
m,k = ak + ba,k + na,k
j=0 (29)
 bω,k = bω,k−1 + nbω,k−1



III. P ROBLEM F ORMULATION AND M EASUREMENTS ba,k = ba,k−1 + nba,k−1
In this section, we will develop a state transition function where nω,k ∈ R3 , na,k ∈ R3 , nbω,k ∈ R3 , and nba,k ∈ R3
that describes the relationship between the current state, the are noise vectors from zero mean Gaussian distributions and
previous state, and the input vectors. Additionally, a mea- covariance matrices of Cω,k , Ca,k , Cbω,k , and Cba,k , respec-
surement function is formulated to describe the relationship tively. From (28) and (29), let us define the following state
between the state vector and the measurement vector. These vector:
functions are crucial for enhancing filter performance. ⊤
xk = qk⊤ p⊤ vk⊤ b⊤ b⊤ ∈ Rmx

k ω,k a,k (30)

A. Model Kinematics with mx = 16 representing the state dimension. Let the

augmented and additive noise vectors with dimensions of
Consider a rigid-body moving in 3D space, with an angular
mnx = 6 and mnw = 16 be defined as:
velocity ω ∈ R3 and an acceleration a ∈ R3 , measured
and expressed w.r.t {B}. Let the position p ∈ R3 and linear ⊤
(
nx,k = n⊤ n⊤ ∈ Rmnx

ω,k a,k
velocity v ∈ R3 of the vehicle be expressed in {W}, while ⊤ (31)
nw,k = 0⊤ ⊤
n⊤ ∈ Rmnw

the vehicle’s orientation in terms of quaternion q ∈ S3 be 10×1 nbω,k ba,k
expressed in {B}. The system kinematics in continuous space Let the input vector uk at time step k be defined as:
can then be expressed as [1], [5]:  ⊤ ⊤

1 uk = ωm,k a⊤m,k ∈ Rm u (32)
3
 q̇ = 2 Γ(ω)q ∈ S


where mu = 6. From (30) and (31), let us define the
ṗ = v ∈ R3 (26)

 augmented state vector as:
3
v̇ = g + Rq (q)a ∈ R

⊤
xak = x⊤ n⊤ ∈ Rm a

k x,k (33)
where
−ω ⊤
 
0 with ma = mx + mnx . Combining (28), (29), (31), (32),
Γ(ω) = ∈ R4×4 and (33), one can re-express the system kinematics in discrete
ω −[ω]×
space as follows:
The kinematics expression in (26) can be re-arranged in a
geometric form in a similar manner to geometric navigation xk = f(xak−1 , uk−1 ) + nw,k−1 (34)
in [1], [21] as follows:
   1   where f(·) : Rma × Rmu → Rmx denotes the state transition
q̇ 2 Γ(ω)q 0 0 0 q matrix.
 ṗ   0 0 I3 0  p 
 v̇  = 
     (27)
0 0 0 g + Rq (q)a   v 
B. VIN Measurement Model
0 0 0 0 0 1
| {z } Consider fw,i ∈ R3 to be the coordinates of the ith feature
M c (q,ω,a)
point in {W} obtained by using a series of stereo camera
As the sensors operate in discrete space, the expression in observations and fb,i ∈ R3 be the coordinates of the ith
(27) is discretized for filter derivation and implementation. feature point in {B} reconstructed using the stereo camera
 6

values of xk and zk , respectively. In this model, the expected

values of the RVs are accounted as the real values. However,
it should be noted that directly calculating these expected
values is not feasible; instead, they are estimated. Let the
estimated expected values of the probabilities P(Xk−1 |Zk−1 )
and P(Xk |Zk−1 ) be x̂k−1|k−1 and x̂k|k−1 , respectively, and
the expected value of P(Zk ) be ẑk|k−1 .

A. Filter Initialization
Step 1. State and Covariance Initialization: At the QNUKF
initialization, we establish the initial state estimate x̂0|0
and the covariance matrix P0|0 . While this step is standard
Fig. 1: Navigation problem visualization in the 6 DoF. in CUKF, managing quaternion-based navigation introduces
unique challenges related to quaternion representation and
covariance. A critical challenge arises from the fact that the
measurement at the kth time step. The model describing the covariance is computed based on differences from the mean.
relationship between each fw,i and fb,i can be expressed as Thereby, straightforward quaternion subtraction is not feasible.
[5], [24]: To address this limitation, we employ the custom quaternion
subtraction introduced in (17) and specifically designed for
fb,i = Rq (qk )⊤ (fw,i − pk ) + nf,i ∈ R3 (35)
quaternion operations. Moreover, quaternions inherently pos-
where nf,i refers to white Gaussian noise associated with each sess three degrees of freedom while having four components,
measurement for all i ∈ {1, 2, . . . , mf } and mf ∈ R denotes requiring adjustments to the covariance matrix to reflect the
the number of feature points detected in each measurement reduced dimensionality. Let the initial estimated state vector be
h i ⊤
step. Note that the quantity of feature points mf may vary defined as x̂0|0 = x̂⊤ ⊤
∈ Rmx (see (30)), where
0|0,q , x̂0|0,−
across images obtained at different measurement steps. Let us 3 mx −4
x̂0|0,q ∈ S and x̂0|0,− ∈ R represent the quaternion
define the set of feature points in {W} as fw , the set of feature
and non-quaternion components of x̂0|0 , respectively, with mx
points in {B} as fb , and the measurement noise vector as nf
being the number of rows in the state vector. The filter is
such that
 h i⊤ initialized as:
⊤ ⊤ ⊤
 fb = fb,1 fb,2 · · · fb,m ∈ R3mf h i⊤
x̂0|0 = x̂⊤ ⊤
∈ Rmx

0|0,q , x̂0|0,−
f


 (38)
 ⊤ ⊤ ⊤
⊤
fw = fw,1 fw,2 · · · fw,m f
∈ R3mf (36)
P0|0 = diag(Px̂0|0,q , P0|0,− ) ∈ R(mx −1)×(mx −1) (39)

⊤

 h i
n = n⊤ n⊤ · · · n⊤

∈ R3mf
f f,1 f,2 f,mf where P0|0,q ∈ R3×3 , P0|0,− ∈ R(mx −4)×(mx −4) , and P0|0
Using (36), the term in (35) can be rewritten in form of a represent the covariance matrices corresponding with the un-
measurement function as follows: certainties of x̂0|0,q , x̂0|0,− , and x̂0|0 , respectively.

fb,k = zk = h(xk , fw ) + nf,k ∈ Rmz (37)

B. Prediction
with The prediction step involves estimating the state of the
nf ∼ N (0, Cf ∈ Rmz ×mz ) system at the next time step leveraging the system’s kinematics
given the initial state (38) and covariance (39). At each k
Note that zk is the measurement vector at the kth time step and
time step, the process begins with augmenting the state vector
mz = 3mf ∈ Z+ is the dimension of the measurement vector.
with non-additive noise terms, a critical step that ensures the
Typically, the camera sensor operates at a lower sampling
filter accounts for the uncertainty which these noise terms
frequency when compared to the IMU sensor. As a result,
introduce. Subsequently, sigma points are generated from the
there may be a timing discrepancy between the availability
augmented state vector and covariance matrix. These sigma
of images and the corresponding IMU data. The conceptual
points effectively represent the distribution P(Xk−1 |Zk−1 ),
illustration of the VIN problem is presented in Fig. 1.
capturing the state’s uncertainty at the previous time step
given the previous observation, see (22). (20). The next phase
IV. QNUKF- BASED VIN D ESIGN involves computing the posterior sigma points by applying
To develop the QNUKF, we will explore how the Conven- the state transition function to each sigma point, thereby
tional UKF (CUKF) [33] functions within the system. Next, representing the distribution P(Xk |Xk−1 ) which is equivalent
we will modify each step to address the CUKF limitations. to P(Xk |Zk−1 ). This distribution reflects the predicted state’s
Stochastic filters, such as UKF models system parameters as uncertainty before the current observation is considered. The
RVs and aim to estimate the expected values of their distribu- predicted state vector is then obtained as the weighted average
tions, which are then used as the estimates of the parameters. of these posterior sigma points. This process is described in
Let Xk and Zk represent the RVs associated with the real detail in this subsection.
 7

Step 2. Augmentation: Prior to calculating the sigma points, to the reduced dimensionality of the quaternion. To resolve this
the state vector and covariance matrix are augmented to limitation, the propose QNUKF modifies δ x̂ak−1,j as follows:
incorporate non-additive process noise. This involves adding q 
rows and columns to the state vector and covariance matrix to δ x̂ak−1,j := a
(ma − 1 + λ)Pk−1|k−1 ∈ Rma −1 (45)
j
represent the process noise. The expected value of the noise
terms, zero in this case, are augmented to the previous state es- Overall, considering (44) and (45), the QNUKF utilizes the
timate x̂k−1|k−1 to form the augmented state vector x̂ak−1|k−1 . modified version of (43) as follows:
Similarly, the last estimated state covariance matrix Pk−1|k−1 
 a
Xk−1|k−1,0 = x̂ak−1|k−1 ∈ Rma
and the noise covariance matrix Cx,k are augmented together 

a
Xk−1|k−1,j = x̂ak−1|k−1 ⊕ δ x̂aj

a

to form the augmented state covariance matrix Pk−1|k−1 . This 


 " #
is formulated as follows: a a
x̂ ⊕ δ x̂


k−1,j,r
:= ak−1|k−1,q ∈ Rm a


x̂k−1|k−1,− + δ x̂ak−1,j,−
h i⊤ 


x̂ak−1|k−1 = x̂⊤ , 0 ⊤
k−1|k−1 mnx ×1 ∈ Rma (40)
a
 Xk−1|k−1,j+m = x̂ak−1|k−1 ⊖ δ x̂aj
= diag(Pk−1|k−1 , Cx,k ) ∈ R(ma −1)×(ma −1) (41)
a  a
Pk−1|k−1 
 " #
x̂ak−1|k−1,q ⊖ δ x̂ak−1,j,r


∈ Rm a ,

:= a


The covariance matrix Cx,k in (41) is defined by: x̂k−1|k−1,− − δ x̂ak−1,j,−






Cx,k = diag(Cω,k , Ca,k ) ∈ Rmnx ×mnx j = {1, 2, . . . , 2(ma − 1)}

(42)
(46)
Step 3. Sigma Points Calculation: Conventionally, sigma Step 4. Propagate Sigma Points: The sigma points
a
points are computed based on the augmented state estimate {Xk−1|k−1,j } are propagated through the state transition func-
and covariance matrix. They serve as a representation for tion, as defined in (34), to calculate the propagated sigma
the distribution and uncertainty of P(xk−1|k−1 |zk−1 ). In other points {Xk|k−1,j }. These points represent the probability dis-
words, the greater the uncertainty of the last estimate reflected tribution P(Xk |Xk−1 ), and are computed as follows:
by a larger last estimated covariance, the more widespread a
Xk|k−1,j = f(Xk−1|k−1,j , uk−1 ) ∈ Rmx (47)
the sigma points will be around the last estimated state.
Reformulating (22) according to the CUKF algorithm, the for all j = {0, 1, 2, . . . , 2(ma − 1)}.
sigma points, crucial for propagating the state through the Step 5. Compute Predicted Mean and Covariance: The pre-
system dynamics, are computed using the following equation: dicted mean x̂k|k−1 and covariance Pk|k−1 are conventionally
a computed using the predicted sigma points by reformulating
= x̂ak−1|k−1 ∈ Rma

Xk−1|k−1,0
(24) and (25) as:


 q 
a
= x̂ak−1|k−1 +
 a
Xk−1|k−1,j (ma + λ)Pk−1|k−1


 2m
Xa
q j x̂k|k−1 = wjm Xk|k−1,j ∈ Rmx (48)
a a a


 Xk−1|k−1,j+ma = x̂k−1|k−1 − (ma + λ)Pk−1|k−1 , j=0

 j
2m


j = {1, 2, . . . , 2ma } Xa
wj (Xk|k−1,j − x̂k|k−1 )(Xk|k−1,j − x̂k|k−1 )⊤
 c 
(43) Pk|k−1 =
Challenges arise when using the sigma points calcula- j=0

tion as described in (43) for quaternion-based navigation + Cw,k ∈ R(mx −1)×(mx −1) (49)
problems due to the differing mathematical spaces in-
where wjm and wjc ∈ R represent the weights associated
volved. For the sake  of brevity, let us define δ x̂ak−1,j :=
 q with the sigma points and are found by (23). Additionally,
a
(ma + λ)Pk−1|k−1 ∈ Rma −1 . It is possible to divide Cw,k ∈ Rmx −1 denotes the process noise covariance matrix,
j
x̂ak−1|k−1 and δ x̂ak−1,j into their attitude and non-attitude parts: which is the covariance matrix of the noise vector nw,k as
x̂k−1|k−1,q ∈ S3 , δ x̂ak−1,j,r ∈ R3 , and x̂ak−1|k−1,− ∈ Rma −4 , defined in (31). Note that the reduced dimensionality of Cw,k ,
δ x̂ak−1,j,− ∈ Rma −4 , as outlined below: similar to Pk|k−1 and P0|0 , is due to the covariance matrices of
quaternion random variables being described by 3×3 matrices,
 h i⊤ reflecting the three degrees of freedom inherent to quaternions.
x̂a a ⊤ a ⊤
k−1|k−1 = (x̂k−1|k−1,q ) , (x̂k−1|k−1,− ) Cw,k is defined as
⊤ (44)
δ x̂ak−1,j = (δ x̂ak−1,j,r )⊤ , (δ x̂ak−1,j,− )⊤
   
09×9 03×3 03×3
Cw,k = 09×9 Cbω,k 03×3  ∈ R(mx −1)×(mx −1) (50)
From (44), given that δ x̂ak−1,j,r is a rotation vector in R3 and
09×9 03×3 Cba,k
x̂ak−1|k−1,q is a quaternion expression in S3 , direct addition
or subtraction is not feasible. To resolve this limitation, we To tailor (48), (49), and (23) for the quaternion-based nav-
employ the custom subtraction (16) and addition (15) opera- igation problem, the following modifications are necessary:
tors, as previously defined. The operation map S3 × R3 → S3 (1) The QNUKF utilizes ma − 1 sigma points, therefore, all
effectively manages the interaction between quaternions and occurrences of ma in (48), (49), and (23) should be replaced
rotation vectors. Additionally, the quaternion-based navigation with ma −1; (2) The average of quaternion components of the
problem inherently possesses ma − 1 degrees of freedom due sigma points in (48) should be computed using the quaternion
 8

QNUKF
IM U Data Pr edict Sigm a QNUKF
Points Does Cam er a Data Yes Cacluate
Exist? M easur m ents
Sigm a Points
QNUK
Calculate
Sigm a
Points No

QNUKF QNUKF QNUKF

Estim ated
Pr edict The Pr edict The Pr edict The
State
State State M esur m ent

QNUKF
Featur e Point Cor r ection
M easur m ents

Fig. 2: Schematic diagram of the proposed QNUKF. First, the sigma points based on the last estimated state vector and its
covariance matrix are calculated. Using these sigma points and IMU measurements, the prediction step is performed. If camera
data exists at that sample point, the correction (also known as update) step is performed. The state and covariance estimation
at that time step is the result of the correction step if camera data exists, and the prediction step if it does not.

weighted average method described in (18). This is necessary and 

since the typical vector averaging method in (48) cannot be λ

w0m = ∈R
λ + (ma − 1)

applied directly to quaternions; (3) The subtraction of quater- 


λ

nion components of the sigma point vector by the quaternion 
 wc = + 1 − α2 + β ∈ R

components of the estimated mean in (49) should utilize the 0
λ + (ma − 1) (55)
custom subtraction method defined in (17). This operation  1
wjm = wjc =

∈R

maps S3 × S3 to R3 , reflecting the fact that quaternions, which

2((m − 1) + λ)


 a
exist in S3 space, cannot be subtracted using conventional


j = {1, . . . , 2(ma − 1)}

vector subtraction techniques.
To facilitate these operations, let us divide x̂k|k−1 and where
Xk|k−1,j to their quaternion (x̂k|k−1,q ∈ S3 , Xk|k−1,j,q ∈
 
Xk|k−1,j,q ⊖ x̂k|k−1,q
S3 ) and non-quaternion (x̂k|k−1,− ∈ Rnx −4 , Xk|k−1,j,− ∈ Xk|k−1,j ⊖ x̂k|k−1 = ∈ Rmx −1
Xk|k−1,j,− − x̂k|k−1,−
Rmx −4 ) components. These divisions can be represented as (56)
follows:

h i⊤ C. Update
x̂k|k−1 = x̂⊤
k|k−1,q x̂ ⊤
k|k−1,− Rm x (51) In the update step, the propagated sigma points are first
h
⊤ ⊤
i⊤ processed through the measurement function to generate
Xk|k−1,j = Xk|k−1,q Xk|k−1,− ∈ Rm x (52) the measurement sigma points, which represent P(Zk |Xk ).
Subsequently, utilizing P(Xk−1 |Zk−1 ), P(Xk |Xk−1 ), and
P(Zk |Xk ), the conditional probability P(Xk |Zk ) is estimated.
Consequently, equations (48), (49), and (23) can be refor- The expected value derived from P(Xk |Zk ) will serve as the
mulated to accommodate these quaternion-specific processes, state estimate for the kth step.
utilizing the divisions formulated in (51) and (52) as follows:
Step 6. Predict Measurement and Calculate Covariance: In
this step, each propagated sigma point Xk|k−1,j , as found in
QWA({Xk|k−1,j,q }, {wjm }) (47), is processed through the measurement function defined
 
 2(m a −1) m in (37) to compute the jth predicted sigma point Zk|k−1,j .
x̂k|k−1 = ∈R x (53)
X 
wjm Xk|k−1,j,− The set {Zk|k−1,j } represent the probability distribution
j=0 P(Zk |Xk ). For all j = {0, 1, . . . , 2(ma −1)}, the computation
for each sigma point is given by:

Zk|k−1,j = h(Xk|k−1,j , fw ) ∈ Rmz (57)

2(ma −1) h
X i The expected value of P(Zk |Xk ) is the maximum likelihood
Pk|k−1 = wjc (Xk|k−1,j ⊖ x̂k|k−1 )(Xk|k−1,j ⊖ x̂k|k−1 )⊤ estimate of the measurement vector. This estimate ẑk|k−1
j=0
is calculated according to (58). Additionally, the estimated
+ Cw,k ∈ R(mx −1)×(mx −1) (54) measurement covariance matrix Pzk ,zk ∈ Rmz ×mz and the
 9

state-measurement covariance matrix Pxk ,zk ∈ R(mx −1)×mz vector in the three-dimensional Euclidean space R3 . To add the
are computed using the following equations: orientation parts of the predicted state and correction vectors,
2(ma −1)
the custom summation operator S3 × R3 → S3 described in
ẑk|k−1 =
X
wjm Zk|k−1,j (58) (15) should be used as follows
j=0 x̂k|k = x̂k|k−1 ⊕ δ x̂k|k−1 (66)
2(ma −1)
where
X
Pzk ,zk = wjc [Zk|k−1,j − ẑk|k−1 ][Zk|k−1,j − ẑk|k−1 ]⊤  
j=0 x̂k|k−1,q ⊕ δ x̂k|k−1,r
x̂k|k−1 ⊕ δ x̂k|k−1 = (67)
+ Cf (59) x̂k|k−1,− + δ x̂k|k−1,−
2(ma −1)
X Step 8. Iterate: Go back to Step 2. and iterate from k →
Pxk ,zk = wjc [Xk|k−1,j ⊖ x̂k|k−1 ][Zk|k−1,j − ẑk|k−1 ]⊤ k + 1.
j=0 The proposed QNUKF algorithm is visually depicted in
(60) Fig. 2. The implementation steps of QNUKF algorithm is
Note that the subtraction operator ⊖ in (60) denotes state dif- summarized and outlined in Algorithm 1.
ference operator and follows (56). Now, the joint distribution Remark 1. The time complexities of the vanilla EKF and UKF
of (Xk , Zk ) is estimated as follows: algorithms are O(m3x ) and O(m3a ), respectively [39]. For nav-
Xk
!
x̂k|k−1
!
Pk|k−1 Pxk ,zk
!! igation tasks, these complexities reduce to O((mx − 1)3 ) and
∼N , (61) O((ma − 1)3 ), respectively, due to the reduced dimensionality
Zk ẑk|k−1 Px⊤k ,zk Pzk ,zk of the quaternion in the state vector. The custom operations
These covariance calculations are necessary to estimate the proposed in QNUKF, such as the one used in (67), all have
probability distribution P(Xk |Zk ) in the next step. complexities less than O((ma − 1)) and occur only once for
Step 7. Update State Estimate: In view of (61) to (19), let us each sigma point. Therefore, QNUKF retains the same overall
reformulate (20) to find the mean and covariance estimate of complexity as any UKF, which is O((ma −1)3 ). The difference
the P(Xk |Zk ) distribution. It is worth noting that the estimated between ma and mx arises because UKF can capture non-
mean is the estimated state vector at this time step. This is additive noise terms by augmenting them into the state vector,
achieved by computing the Kalman gain Kk ∈ R(mx −1)×mz whereas EKF linearizes them into an additive form.
given the matrices found in (59) and (60).
Kk = Pxk ,zk Pz⊤k ,zk ∈ R(mx −1)×mz (62) V. I MPLEMENTATION AND R ESULTS
To evaluate the effectiveness of the proposed QNUKF, the
To find the updated covariance estimate of the state distribution
algorithm is tested using the real-world EuRoC dataset of
Pk|k in the QNUKF, the conventional formula provided in (63)
drone flight in 6 DoF [40]. For video of the experiment, visit
is utilized:
the following link. This dataset features an Asctec Firefly hex-
Pk|k = Pk|k−1 − Kk Pzk ,zk Kk⊤ (63) rotor Micro Aerial Vehicle (MAV) flown in a static indoor en-
vironment, from which IMU, stereo images, and ground truth
However, computing the updated estimated state vector x̂k|k data were collected. The stereo images are two simultaneous
requires careful consideration. Define the correction vector monochrome images obtained via an Aptina MT9V034 global
δ x̂k|k−1 as: shutter sensor with a 20 Hertz frequency. Linear acceleration
δ x̂k|k−1 := Kk (zk − ẑk|k−1 ) ∈ Rmx −1 (64) and angular velocity of the MAV were measured by an
ADIS16448 sensor with a 200 Hertz frequency. Note that the
Recall that zk refers to camera feature measurements in {B}. data sampling frequency of the camera and IMU are different.
Conventionally, the correction vector calculated in (64), is Consequently, the measurement data does not necessarily exist
added to the predicted state vector from (53) to update the at each IMU data sample point. To incorporate this, the system
state. However, for a quaternion-based navigation problem, only performs the update step when image data is retrieved,
the dimensions of δ x̂k|k−1 and x̂k|k−1 do not match, with and sets x̂k|k equal to the predicted estimated state vector
the former being mx − 1 and the latter mx . To address this, x̂k|k−1 when image data is unavailable.
let us divide δ x̂k|k−1 into its attitude (δ x̂k|k−1,r ∈ R3 ) and
non-attitude (δ x̂k|k−1,− ∈ Rm⋆ −4 ) components: A. The Proposed QNUKF Output Performance
i⊤
The feature point count mf is not constant at each mea-
h
δ x̂k|k−1 = δ x̂⊤
k|k−1,r δ x̂ ⊤
k|k−1,− (65)
surement step, to compensate for this, we set the measurement
From (65) with (51), we observe that the non-quaternion com- noise covariance to:
ponents of x̂k|k−1 and δ x̂k|k−1 are dimensionally consistent
Cf = c2f Imf (68)
and can be combined using the conventional addition operator.
However, the orientation components of x̂k|k−1 and δ x̂k|k−1 where mf is based on the number of feature points at each
do not match dimensionally. The former is represented by a measurement step and cf is a scaler tuned beforehand. To
quaternion within the S3 space, while the latter is a rotation ensure the covariance matrices remain symmetric and avoid
 10

Algorithm 1 Quaternion Navigation Unscented Kalman Filter (ma − 1), α = 10−4 , and β = 2. The IMU bias noise
Initialization: covariances are determined by the standard deviations (std)
1: Set x̂0|0 ∈ Rmx −4 , see (30), and P0|0 ∈ of the bias white noise terms, set to 0.01% of their av-
(mx −1)×(mx −1) erage values to account for the slow random walk relative
R .
2: Set k = 0 and select the filter design parameters λ, to the IMU sampling rate. These values are configured as:
α, β ∈ R, along with the noise covariance matrices Cba,k = 10−8 diag(0.00222 , 0.02082 , 0.07582 ) (m/s2 )2 and
Ca,k , Cω,k , Cba,k , Cbω,k ∈ R3×3 , and the noise variance Cbω,k = 10−8 diag(0.01472 , 0.10512 , 0.09302 ) (rad/s)2 . The
cf ∈ R. std of the additive white noise terms are set to 1% of
3: Calculate Cx,k , Cw,k , and Cf as expressed in (42), (50), the measured vectors (i.e., linear acceleration and angular
and (68), respectively. velocity) such that the covariance matrices are Cω,k =
while IMU data exists 10−4 diag(0.13562 , 0.03862 , 0.02422 ) (m/s2 )2 and Ca,k =
10−4 diag(9.25012 , 0.02932 , −3.36772 ) (rad/s)2 . The mea-
/* Prediction */ h i⊤ surement noise std is calculated using the measurement vectors

a ⊤ ⊤ (m −1)×1
x̂k−1|k−1 = "x̂k−1|k−1 , 06×1 ∈ R a

from the V1 01 hard dataset [40], recorded in the same

#
4: Pk−1|k−1 0(mx −1)×mnx room as the validation V1 02 medium dataset [40], but with
a
Pk−1|k−1 =

different drone paths and complexity speeds. Its value is found

0mnx ×(mx −1) Cx,k
a
5: Xk−1|k−1,0 = x̂ak−1|k−1 as cf = 0.099538 m. For each set of stereo images obtained,
6: for j = {1, 2 . . .  feature points are identified using the Kanade-Lucas-Tomasi
q a − 1)}
, (m 
(KLT) algorithm [41]. An example of the feature match results
a a
δ x̂k−1,j := (ma − 1 + λ)Pk−1|k−1 , see (45)
j from this operation is depicted in Fig. 3.
a
Xk−1|k−1,j = x̂ak−1|k−1 ⊕ δ x̂aj , see (46)
Xk−1|k−1,j+ma = x̂ak−1|k−1 ⊖ δ x̂aj , see (46)
a

end for
7: for j = {1, 2 . . . , (ma − 1)}
a
Xk|k−1,j = f(Xk−1|k−1,j , uk−1 )
end for, see (47)
QWA({Xk|k−1,j,q }, {wjm })
 
 2(m a −1)
8: x̂k|k−1 =  , see (53)
X 
wjm Xk|k−1,j,−
j=0
P2(m −1)
9: Pk|k−1 = j=0 a [wjc (Xk|k−1,j ⊖ x̂k|k−1 )(Xk|k−1,j ⊖
x̂k|k−1 )⊤ ] + Cw,k , see (54)
/* Update step */
10: for j = {0, 1, 2 . . . , (ma − 1)} Fig. 3: Matched feature points from the left and right views
a
Zk|k−1,j = h(Xk|k−1,j , fw ) of the EuRoC dataset [40].
end for, see (57)
P2(ma −1) m
11: ẑk|k−1 = wj Zk|k−1,j , see (58) Using triangulation [42], the 2D matched points are pro-
Pj=0
2(ma −1) c jected to the 3D space, representing the feature points in {W}.
12: Pzk ,zk = j=0 wj [Zk|k−1,j − ẑk|k−1 ][Zk|k−1,j −
ẑk|k−1 ]⊤ + Cf , see (59) Using the S3 × S3 → R3 subtraction operator defined in (17),
P2(ma −1) c let us define the orientation estimation error re,k as:
13: Pxk ,zk = j=0 wj [Xk|k−1,j ⊖ x̂k|k−1 ][Zk|k−1,j −
ẑk|k−1 ]⊤ , see (60) re,k := qk ⊖ q̂k (70)
= Pxk ,zk Pz⊤k ,zk

 Kk  ⊤
where re,k = re1,k re2,k re3,k ∈ R3 , with each


k|k−1 = K(zk − ẑk|k−1 )
δ x̂
14: , see (67) component rei,k ∈ R representing a dimension of the error
x̂ k|k = x̂k|k−1 ⊕ δ x̂k|k−1
vector. Similarly, let us define the position and linear velocity



Pk|k = Pk|k−1 − Kk Pzk ,zk Kk⊤

estimation errors at the kth time step, represented by pe,k and
15: k = k + 1 ve,k , respectively, as:
end while
pe,k := pk − p̂k (71)
ve,k := vk − v̂k (72)
numerical issues, for any P ∈ RmP ×mP , the Symmetrize
function is defined as Symmetrize : RmP ×mP → RmP ×mP :  ⊤
where pe,k = pe1,k pe2,k pe3,k ∈ R3 and ve,k =
P + P⊤  ⊤ 3
Symmetrize(P ) := ∈ RmP ×mP . (69) ve1,k ve2,k ve3,k ∈ R . The robustness and effective-
2 ness of the proposed QNUKF have been confirmed through
This function is applied to all single-variable covariance experimental results on the EuRoC V1 02 medium dataset
matrices after they are calculated according to the QNUKF. [40] as depicted in Fig. 4 and with the initial estimates for the
The filter parameters are configured as follows: λ = 3 − state vector and covariance matrix being detailed as follows:
 11

Dataset Trajectory

2.5

||re,k|| [rad.m]
2

1.5
2.4
1
2.2
0.5

0
2 0 20 40 60 80

1.8 2.5

2
Z (m)

1.6

||pe,k|| [m]
1.5
1.4
1

1.2 0.5

1 0 20 40 60 80

6
0.8
4 5

||ve,k|| [m/s]
3 4
2
2 1.5 3
1
1 0.5
2
0
0 -0.5
-1 1
-1 -1.5
Y (m) -2
-2 X (m) 0 20 40 60 80
-2.5
Time [s]

Fig. 4: Validation results of the proposed QNUKF using the EuRoC V1 02 medium dataset [40]. On the left, the navigation
trajectory of the drone moving in 3D space is shown with a solid black line, and the drone orientation with respect to x, y, and
z axes is represented by red, green, and blue dashed lines, respectively. On the right, the magnitudes of the orientation error
vector ||re,k ||, position error vector ||pe,k ||, and linear velocity error vector ||ve,k || are plotted over time in solid red lines.

B. Comparison with State-of-the-art Literature

For video of the comparison, visit the following link. To
evaluate the efficacy of the proposed filter, we compared its
performance with the vanilla Extended Kalman Filter (EKF),
the standard tool in non-linear estimation problems as well
as the Multi-State Constraint Kalman Filter (MSCKF) [43],
which addresses the observability issues of the EKF caused
by linearization. Both models were implemented to operate
with the same state vectors (as defined in (30)), state transition
function (as defined in (34)), and measurement function (as de-
fined in (37)). Additionally, their noise covariance parameters,
as well as initial state and covariance matrices, were set to
Fig. 5: Estimation errors in rotation vector (left), position identical values in each experiment for all the filters to ensure
(middle), and linear velocity (right) components, from top to a fair comparison. In light of (70), (71), and (72), let us define
bottom of the proposed QNUKF. the estimation error at time step k denoted by ek as:
ek = ||re,k || + ||pe,k || + ||ve,k || ∈ R (73)

x̂0:0 = [0.1619, 0.7900, −0.2053, 0.5545, 0.6153, 2.0967, Using (73), the Root Mean Square Error (RMSE) is calculated
0.7711, 0, 0, 0, −0.0022, 0.0208, 0.0758, −0.0147, 0.1051, as: v
mk
0.0930]⊤ and P0|0 = diag(80I3 , 10I3 , 70I3 , 10I6 ). On the left
u
u 1 X
side of Fig. 4, the trajectory and orientation of the drone RMSE = t e2k
mk
k=1
during the experiment are depicted. On the right side, the
magnitudes of the orientation, position, and linear velocity where mk is the total number of time steps. The steady state
error vectors are plotted over time. The results demonstrate root mean square error (SSRMSE) is defined similarly to
that all errors converge to near-zero values and recover from RMSE but calculated over the last 20 seconds of the experi-
initial large errors rapidly, indicating excellent performance. ment. Table II compares the performance of the EKF, MSCKF,
To examine the filter’s performance in detail, each component and QNUKF across three different EuRoC [40] datasets us-
of the estimation error is plotted against time. These plots are ing RMSE and SSRMSE as metrics. Experiments 1, 2, and
displayed in Fig. 5, showing excellent performance in tracking 3 correspond to the V1 02 medium, V1 03 difficult, and
each component of orientation, position, and linear velocity. V2 01 easy datasets, respectively, with the best RMSE and
 12

TABLE II: Performance Comparison of QNUKF (proposed) sampling rates, handling kinematic nonlinearities effectively,
vs EKF and MSCKF (literature) on EuRoC Datasets. and avoiding orientation singularities, such as those com-
Filter Metric Experiment 1 Experiment 2 Experiment 3 mon in Euler angle-based filters. The proposed Quaternion-
RMSE 0.952955 0.831777 1.411102 based Navigation Unscented Kalman Filter (QNUKF) relies
EKF
SSRMSE 0.123161 0.160883 0.263435 on onboard Visual-Inertial Navigation (VIN) sensor fusion of
RMSE 0.866786 1.175951 1.454449 data obtained by a stereo camera (feature observations and
MSCKF
SSRMSE 0.074154 0.062882 0.075329 measurements) and a 6-axis Inertial Measurement Unit (IMU)
QNUKF
RMSE 0.331952 0.275067 0.265037 (angular velocity and linear acceleration). The performance of
SSRMSE 0.059464 0.051633 0.060865 the proposed algorithm was validated using real-world dataset
of a drone flight travelling in GPS-denied regions where stereo
camera images and IMU data were collected at low sampling
SSRMSE values in each experiment. The proposed QNUKF rate. The results demonstrated that the algorithm consistently
algorithm consistently outperformed the other filters across all achieved excellent performance, with orientation, position, and
experiments and metrics. While the MSCKF showed better linear velocity estimation errors quickly converging to near-
performance than the EKF, it lacked consistency, with RMSE zero values, despite significant initial errors and the use of low-
values ranging from 0.866786 to 1.454449. In contrast, the cost sensors with high uncertainty. The proposed algorithm
QNUKF maintained a more stable performance, with RMSE consistently outperformed both baseline and state-of-the-art
values tightly clustered between 0.265037 and 0.331952. filters, as demonstrated by the three experiments conducted
Let us examine the orientation, position, and linear velocity in two different rooms.
estimation error trajectories for all the filters in Experiment As the next step, we will investigate the effectiveness
2 as a case study. As depicted in Fig. 6, the QNUKF out- of adaptive noise covariance tuning on QNUKF to mitigate
performed the other filters across all estimation errors. The this issue. Additionally, reworking QNUKF to address si-
EKF exhibited a slow response in correcting the initial error, multaneous localization and mapping (SLAM) for UAVs is
while the MSCKF corrected course more quickly but initially a consideration for future work, as it represents a natural
overshot compared to the EKF. In contrast, the QNUKF progression from navigation.
responded rapidly and maintained smaller steady-state errors
when compared with EKF and MSCKF. R EFERENCES
[1] H. A. Hashim, “GPS-denied navigation: Attitude, position, linear veloc-
ity, and gravity estimation with nonlinear stochastic observer,” in 2021
American Control Conference (ACC). IEEE, 2021, pp. 1149–1154.
[2] Y. Xu, Y. S. Shmaliy, S. Bi, X. Chen, and Y. Zhuang, “Extended
kalman/ufir filters for uwb-based indoor robot localization under time-
varying colored measurement noise,” IEEE Internet of Things Journal,
vol. 10, no. 17, pp. 15 632–15 641, 2023.
[3] H. A. Hashim, A. E. Eltoukhy, and A. Odry, “Observer-based controller
for VTOL-UAVs tracking using direct vision-aided inertial navigation
measurements,” ISA transactions, vol. 137, pp. 133–143, 2023.
[4] Q. Wang, H. Luo, H. Xiong, A. Men, F. Zhao, M. Xia, and C. Ou,
“Pedestrian dead reckoning based on walking pattern recognition and
online magnetic fingerprint trajectory calibration,” IEEE Internet of
Things Journal, vol. 8, no. 3, pp. 2011–2026, 2020.
[5] H. A. Hashim, M. Abouheaf, and M. A. Abido, “Geometric stochastic
filter with guaranteed performance for autonomous navigation based on
IMU and feature sensor fusion,” Control Engineering Practice, vol. 116,
p. 104926, 2021.
[6] O. Asraf, F. Shama, and I. Klein, “Pdrnet: A deep-learning pedestrian
dead reckoning framework,” IEEE Sensors Journal, vol. 22, no. 6, pp.
4932–4939, 2021.
[7] C. Jiang, Y. Chen, C. Chen, J. Jia, H. Sun, T. Wang, and J. Hyyppa,
“Implementation and performance analysis of the pdr/gnss integration
on a smartphone,” GPS Solutions, vol. 26, no. 3, p. 81, 2022.
[8] H. A. Hashim, L. J. Brown, and K. McIsaac, “Nonlinear stochastic
Fig. 6: Estimation errors in orientation (top), position (middle), attitude filters on the special orthogonal group 3: Ito and stratonovich,”
and linear velocity (bottom) for the EKF (red thick solid line), IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 49,
MSCKF (green solid line), and proposed QNUKF (blue dashed no. 9, pp. 1853–1865, 2018.
[9] S. Carletta, P. Teofilatto, and M. S. Farissi, “A magnetometer-only atti-
line). tude determination strategy for small satellites: Design of the algorithm
and hardware-in-the-loop testing,” Aerospace, vol. 7, no. 1, p. 3, 2020.
[10] H. A. Hashim, “Systematic convergence of nonlinear stochastic estima-
tors on the special orthogonal group SO (3),” International Journal of
VI. C ONCLUSION Robust and Nonlinear Control, vol. 30, no. 10, pp. 3848–3870, 2020.
[11] D. Scaramuzza and et al., “Vision-controlled micro flying robots: from
In this paper, we investigated the orientation, position, and system design to autonomous navigation and mapping in GPS-denied
linear velocity estimation problem for a rigid-body navigating environments,” IEEE Robotics & Automation Magazine, vol. 21, no. 3,
in three-dimensional space with six degrees-of-freedom. A ro- pp. 26–40, 2014.
[12] H.-P. Tan, R. Diamant, W. K. Seah, and M. Waldmeyer, “A survey of
bust Unscented Kalman Filter (UKF) is developed, incorporat- techniques and challenges in underwater localization,” Ocean Engineer-
ing quaternion space, ensuring computational efficiency at low ing, vol. 38, no. 14-15, pp. 1663–1676, 2011.
 13

[13] G. P. Roston and E. P. Krotkov, Dead Reckoning Navigation For Walking odometer, and rgb-d camera,” IEEE Transactions on Instrumentation
Robots. Department of Computer Science, Carnegie-Mellon University, and Measurement, vol. 72, pp. 1–12, 2022.
1991. [29] A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman
[14] H. Zhou, Y. Zhao, X. Xiong, Y. Lou, and S. Kamal, “IMU dead- filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE
reckoning localization with rnn-iekf algorithm,” in 2022 IEEE/RSJ international conference on robotics and automation. IEEE, 2007, pp.
International Conference on Intelligent Robots and Systems (IROS). 3565–3572.
IEEE, 2022, pp. 11 382–11 387. [30] G. Huang, “Visual-inertial navigation: A concise review,” in 2019
[15] H. A. Hashim, A. E. Eltoukhy, and K. G. Vamvoudakis, “UWB ranging international conference on robotics and automation (ICRA). IEEE,
and IMU data fusion: Overview and nonlinear stochastic filter for inertial 2019, pp. 9572–9582.
navigation,” IEEE Transactions on Intelligent Transportation Systems, [31] A. T. Erdem and A. Ö. Ercan, “Fusing inertial sensor data in an extended
2023. kalman filter for 3d camera tracking,” IEEE Transactions on Image
[16] S. Zheng, Z. Li, Y. Liu, H. Zhang, and X. Zou, “An optimization-based Processing, vol. 24, no. 2, pp. 538–548, 2014.
UWB-IMU fusion framework for UGV,” IEEE Sensors Journal, vol. 22, [32] M. B. Khalkhali, A. Vahedian, and H. S. Yazdi, “Multi-target state
no. 5, pp. 4369–4377, 2022. estimation using interactive kalman filter for multi-vehicle tracking,”
[17] H. H. Helgesen and et al., “Inertial navigation aided by ultra-wideband IEEE Transactions on Intelligent Transportation Systems, vol. 21, no. 3,
ranging for ship docking and harbor maneuvering,” IEEE Journal of pp. 1131–1144, 2019.
Oceanic Engineering, vol. 48, no. 1, pp. 27–42, 2022. [33] S. Haykin, Kalman filtering and neural networks. John Wiley & Sons,
[18] W. You, F. Li, L. Liao, and M. Huang, “Data fusion of UWB and IMU 2004.
based on unscented kalman filter for indoor localization of quadrotor [34] C. Jiang and Q. Hu, “Iterative pose estimation for a planar object
uav,” IEEE Access, vol. 8, pp. 64 971–64 981, 2020. using virtual sphere,” IEEE Transactions on Aerospace and Electronic
[19] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,” Systems, vol. 58, no. 4, pp. 3650–3657, 2022.
in Sensor fusion IV: control paradigms and data structures, vol. 1611. [35] H. A. Hashim, “Special orthogonal group SO(3), euler angles, angle-
Spie, 1992, pp. 586–606. axis, rodriguez vector and unit-quaternion: Overview, mapping and
[20] A. Myronenko and X. Song, “Point set registration: Coherent point drift,” challenges,” arXiv preprint arXiv:1909.06669, 2019.
IEEE transactions on pattern analysis and machine intelligence, vol. 32, [36] F. L. Markley, Y. Cheng, J. L. Crassidis, and Y. Oshman, “Quaternion
no. 12, pp. 2262–2275, 2010. averaging,” 2007.
[21] H. A. Hashim, “A geometric nonlinear stochastic filter for simultaneous [37] Y. Song, N. Sebe, and W. Wang, “Fast differentiable matrix square root
localization and mapping,” Aerospace Science and Technology, vol. 111, and inverse square root,” IEEE Transactions on Pattern Analysis and
p. 106569, 2021. Machine Intelligence, 2022.
[22] Z. Bai, H. Xu, Q. Ding, and X. Zhang, “Side-scan sonar image [38] S. J. Julier and J. K. Uhlmann, “New extension of the Kalman filter
classification with zero-shot and style transfer,” IEEE Transactions on to nonlinear systems,” in Signal Processing, Sensor Fusion, and Target
Instrumentation and Measurement, 2024. Recognition VI, I. Kadar, Ed., vol. 3068, International Society for Optics
[23] J. A. Christian and S. Cryan, “A survey of lidar technology and its use and Photonics. SPIE, 1997, pp. 182 – 193.
in spacecraft relative navigation,” in AIAA Guidance, Navigation, and [39] G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “On the complexity
Control (GNC) Conference, 2013, p. 4641. and consistency of ukf-based slam,” in 2009 IEEE International Con-
[24] Y. Wei and Y. Xi, “Optimization of 3-d pose measurement method ference on Robotics and Automation, 2009, pp. 4401–4408.
based on binocular vision,” IEEE Transactions on Instrumentation and [40] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W.
Measurement, vol. 71, pp. 1–12, 2023. Achtelik, and R. Siegwart, “The EuRoC micro aerial vehicle datasets,”
[25] D. Murray and J. J. Little, “Using real-time stereo vision for mobile The International Journal of Robotics Research, vol. 35, no. 10, pp.
robot navigation,” autonomous robots, vol. 8, pp. 161–171, 2000. 1157–1163, 2016.
[26] T. Huntsberger, H. Aghazarian, A. Howard, and D. C. Trotz, “Stereo [41] J. Shi and Tomasi, “Good features to track,” in 1994 Proceedings of
vision–based navigation for autonomous surface vessels,” Journal of IEEE Conference on Computer Vision and Pattern Recognition, 1994,
Field Robotics, vol. 28, no. 1, pp. 3–18, 2011. pp. 593–600.
[27] J. Kim and S. Sukkarieh, “Real-time implementation of airborne inertial- [42] R. Hartley and A. Zisserman, Multiple view geometry in computer vision.
slam,” Robotics and Autonomous Systems, vol. 55, no. 1, pp. 62–71, Cambridge university press, 2003.
2007. [43] K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar,
[28] J. Huang, S. Wen, W. Liang, and W. Guan, “Vwr-slam: Tightly cou- C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry for
pled slam system based on visible light positioning landmark, wheel fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 3,
no. 2, pp. 965–972, 2018.