Vision aided INS

(1)

Vision aided INS

Comparing the UKF and EKF

Ivan Flatval

Thesis submitted for the degree of Master in Science in Electrical Engineering

30 credits

Department of Physics

Faculty of mathematics and natural sciences

UNIVERSITY OF OSLO

(2)

(3)

Vision aided INS

Comparing the UKF and EKF

Ivan Flatval

(4)

c

2018 Ivan Flatval Vision aided INS

http://www.duo.uio.no/

(5)

Abstract

This thesis contains a description of the Inertial Navigation System (INS) for a flying platform, where the the INS gets external measurements from a camera instead of the more traditional GPS. The camera uses the relative motion between the platform and stationary landmarks in the inertial reference frame to aid the INS.

The Unscented Kalman Filter (UKF) and the Extended Kalman Filter (EKF) are used for the state estimations. The performances of the two filters are tested and compared side-by-side, and the effects of having more than one landmark for the camera to focus on is also looked at.

The system are simulated and tested in Matlab with Monte Carlo simulations. The process are discretizized with the first order Euler method.

A normalized pinhole camera model is used for the camera model.

The UKF seems to have more accurate estimation capabilities than the EKF, especially with regards to the covariance estimations, and two landmarks seems to improve some estimations substantially. The UKF seems to be able to estimate the states relatively accurately, although there are some issues in the estimations when the platform is accelerating.

(6)

Preface

This thesis is done as the final work towards a master’s degree in Science in Electrical Engineering at the University of Oslo.

I would like to express appreciation to my supervisor Oddvar Hallingstad for the help and guidance he has provided throughout my master studies at UiO, and especially for this thesis.

(7)

List of Figures

1 Flow chart trajectory generator and sensor data to measurements

z¯_k and input

u, with the measurement noise¯ w_k and

the error equations from the INS. . . 11

2 Illustration of a point P projected on an image, for a pinhole camera, with the coordinates of the point in the camera reference frame ¯ ρ^c, the pinhole image coordinates sx,sy, and focal length f. . . 14

3 Point P as a stationary point in the inertial reference frame Fⁿ, seen from the inertial reference framen. The camera and body frame coincide. Where ¯ pⁿ is the position of the platform, ¯rⁿ is the position of the point P, and ¯ ρ^c is the position of the point P in the cameracand bodybreference frames. . 15

4 Trajectory for the numerical accuracy test. Position ¯ pⁿ, velocity v¯ⁿ and acceleration a¯ⁿ. . . 36

5 Constant velocity trajectory through time and space. . . 39

6 Acceleration trajectory through time and space. . . 40

7 Swaying trajectory through time and space. . . 41

8 Error plot for the positionpⁿ_xand velocityvⁿ_xestimate for the numerical test, with the sampling frequencies 50Hz, 100Hz and 200Hz, known initial conditions, without noise and no measurement updates . . . 44

9 Monte Carlo error plots for the velocity pⁿ_x [m], for the constant velocity trajectory, with a camera update frequency of 1Hz. . . 47

10 Monte Carlo error plots for the velocity pⁿ_x [m], for the constant velocity trajectory, with Nmc =100. . . 48

11 Monte Carlo error plots for the velocity pⁿ_x [m], for the constant velocity trajectory, with Nmc =1000. . . 49

12 Monte Carlo error plots for the velocity pⁿ_y [m], for the constant velocity trajectory, with Nmc =100. . . 50

(10)

13 Monte Carlo error plots for the velocity pⁿ_y [m], for the constant velocity trajectory, withNmc =1000. . . 51 14 Monte Carlo error plots for the velocity pⁿ_z [m], for the con-

stant velocity trajectory, withNmc =100. . . 52 15 Monte Carlo error plots for the velocity pⁿ_z [m], for the con-

stant velocity trajectory, withNmc =1000. . . 53 16 Monte Carlo error plots for the velocity vⁿ_x [m/s], for the

constant velocity trajectory, with a camera update frequency of 1Hz. . . 54 17 Monte Carlo error plots for the velocity vⁿ_x [m/s], for the

constant velocity trajectory, with a camera update frequency of 10Hz. . . 55 18 Monte Carlo error plots for the velocity vⁿ_y [m/s], for the

constant velocity trajectory. . . 56 19 Monte Carlo error plots for the velocity vⁿ_z [m/s], for the

constant velocity trajectory. . . 57 20 Monte Carlo error plots for the roll orientationλ_φ[rad], for

the constant velocity trajectory. . . 58 21 Monte Carlo error plots for the pitch orientation λ_θ [rad],

for the constant velocity trajectory. . . 59 22 Monte Carlo error plots for the yaw orientationλ_ψ[rad], for

the constant velocity trajectory. . . 60 23 Monte Carlo error plots for the velocity pⁿ_x [m], for the ac-

celeration trajectory. . . 62 24 Monte Carlo error plots for the velocity pⁿ_y [m], for the ac-

celeration trajectory. . . 63 25 Monte Carlo error plots for the velocity pⁿ_z [m], for the ac-

celeration trajectory. . . 64 26 Monte Carlo error plots for the velocity vⁿ_x [m/s], for the

acceleration trajectory. . . 65

(11)

27 Monte Carlo error plots for the velocity vⁿ_y [m/s], for the acceleration trajectory. . . 66 28 Monte Carlo error plots for the velocity vⁿ_z [m/s], for the

acceleration trajectory. . . 67 29 Monte Carlo error plots for the roll orientationλ_φ[rad], for

the acceleration trajectory. . . 68 30 Monte Carlo error plots for the pitch orientation λ_θ [rad],

for the acceleration trajectory. . . 69 31 Monte Carlo error plots for the yaw orientationλ_ψ[rad], for

the acceleration trajectory. . . 70 32 Monte Carlo error plots for the positionpⁿ_x[m], for the sway

trajectory, with a prediction frequency of 50Hz. . . 73 33 Monte Carlo error plots for the positionpⁿ_x[m], for the sway

trajectory, with a prediction frequency of 100Hz. . . 74 34 Monte Carlo error plots for the positionpⁿ_y[m], for the sway

trajectory, with a prediction frequency of 100Hz. . . 75 35 Monte Carlo error plots for the positionpⁿ_z [m], for the sway

trajectory, with a prediction frequency of 100Hz. . . 76 36 Monte Carlo error plots for the velocity vⁿ_x [m/s], for the

sway trajectory, with a prediction frequency of 100Hz. . . 77 37 Monte Carlo error plots for the velocity vⁿ_y [m/s], for the

sway trajectory, with a prediction frequency of 100Hz. . . 78 38 Monte Carlo error plots for the velocity vⁿ_z [m/s], for the

sway trajectory, with a prediction frequency of 100Hz. . . 79 39 Monte Carlo error plots for the roll orientationλ_φ[rad], for

the sway trajectory, with a prediction frequency of 100Hz. . 80 40 Monte Carlo error plots for the pitch orientation λ_θ [rad],

for the sway trajectory, with a prediction frequency of 100Hz. 81 41 Monte Carlo error plots for the yaw orientationλ_ψ[rad], for

the sway trajectory, with a prediction frequency of 100Hz. . 82

(12)

List of Algorithms

1 Input . . . 28

2 Extended Kalman Filter (EKF) . . . 29

3 EKF Complex Step Differentiation . . . 30

4 Unscented Kalman Filter (UKF) . . . 31

5 UKF Weights . . . 31

6 UKF Sigma Points (Symmetrical Extended Sigma Set) . . . . 32

7 UKF Prediction . . . 32

8 UKF Update . . . 32

(13)

1 Introduction

For a flying platform to know its own position and orientation it is depen- dent on internal and external measurements. The classical internal measurement method is by using a Inertial Navigation System (INS). The INS uses measurements from an accelerometer to measure the forces acting on the system, and a gyroscope to measure the angular velocity of the system. These measurements have no external reference points, which means that if the measurements have some inherent errors it will diverge from its true position. That will happen because the error in the measurements accumulates in the integration procedures from the forces acting on the platform to its velocity and finally its position, and similarly from its angular velocity to its orientation.

The classical way of combating the divergence is by adding measurements from an external source, such as an Global Navigation Satellite Sys- tem (GNSS). The most common, and widely used GNSS is the United State owned Global Positioning System (GPS). The GPS will give direct position estimates, and by that aid the INS in its position estimates. The problem with a GPS is that it needs direct line of sight to at least three satellites for it to give measurements. Thus it is not always possible to use the GPS for external measurements, such that if the platform is flying indoors, or in an area where there for some reason are not a sufficient amount of satellites within direct line of sight.

Another way of getting external measurements, if GPS measurements is not available or desirable, is by mounting a camera on the flying platform. The camera can then focus on one or more stationary point(s) in the environment (as seen from the inertial reference frame), and use the relative motion between the stationary points and the platform to aid the INS in its position and orientation estimates. The camera will be depen- dent on having the stationary points sufficiently close with regards to the camera resolution, and have them within its field of view. The optimal environment for a camera aided INS could therefore be thought to be inside

(14)

buildings, and not in big open spaces.

One of the most common filters for the non-linear INS estimations is the Extended Kalman Filter (EKF). The EKF is a modified Kalman Filter for use in non-linear systems, were the EKF linearizes about an estimate of the current mean and covariance. The EKF has historically been shown to lose its accuracy with highly non-linear system [7].

The Unscented Kalman Filter (UKF) is another modified Kalman Filter.

This filter uses a different approach with regards to the non-linear systems, it uses the unscented transformation which propagates the meanand the covariance through non-linear transformations. The UKF has shown to be somewhat more accurate in its estimations than the EKF [8].

This thesis builds on the work [1] by Fredrikstad, where Fredrikstad used the Extended Kalman Filter for estimating the states for a flying platform, with one landmark visible to the camera.

The goal of this thesis is to investigate how well the UKF estimates the states compared to the EKF. And to see the effects of having more than one landmark for the camera to focus on.

(15)

2 Extended and Unscented Kalman Filter

This section describes a way of discretizizing the non-linear process such that it can be simulated, and gives a brief explanation of the Extended Kalman Filter and the Unscented Kalman Filter.

2.1 Discretization of Non-Linear Stochastic Processes

The systems process is inherently in a continuous form, while the measurement is discrete [4]. The non-linear stochastic process must be in a discrete form to be simulated on a computer. This section looks at a way of doing this using the Euler method. The Euler method is a first order numerical procedure for solving ordinary differential equation with a given initial value. The method gives a local error proportional to the square of the step size, while the global error is proportional to the step size.

The continuous non-linear process function is defined as

¯˙ x =

¯f(

¯x, u,¯

v¯),

x¯(t₀) ∼ N(ˆ x¯₀, ˆP₀),

v¯(t)∼ N(

¯0, ˜Q·δ(t−τ)) (2.1) with the state vector

¯x, input vector

u, and process noise vector¯

v, where¯ the initial state

¯x(t0) is drawn from a normal distribution with mean ˆ x¯₀, and covariance ˆP0, where ˆP0 is the initial covariance matrix. The noise is also drawn from a normal distribution with mean 0, and a covariance Q˜ ·δ(t−τ), where ˜Q is the spectral density for the process noise, and δ(t−τ)is the dirac delta function.

The non-linear measurement function is discrete per definition, and it can be defined as

¯z_k =

¯h( x¯_k,

w¯_k),

w¯_k ∼ N(

¯0,R_k·δ_kl) (2.2) where

¯xk is the discrete state vector,

w¯kis the discrete measurement noise vector,R_kis the measurements noise covariance matrix, andδ_kl is the kronecker delta.

(16)

To be able to simulate the system in a computer the process also needs to be on a discrete form, such that

¯x_k+1=

¯f_k(

¯x_k, u¯_k,

v¯_k)_,

v¯_k ∼N(

0,¯ Q_k·δ_kl) _(2.3) whereQk is the discrete process noise covariance matrix.

It is desirable for the discrete process noise

v¯_k to have the same effect on

x¯_k →

x¯_k+1 as the continuous process noise

v¯(t) has on

x¯(k·∆) → x¯((k+1)·∆). Where∆is the sub-interval used for prediction.

Ifx¯^d is the solution to the deterministic differential equation

¯˙ x^d=

¯f( x¯^d,

u,¯ 0¯),

¯x^d(t0) = ˆ

¯x₀ (2.4)

and it is assumed that the process noise is small, such that its contribution can be expressed with the first order in the Taylor series

¯˙ x=

¯f( x,¯

u,¯

¯v),

x¯(t_k) =

x¯_k (2.5)

¯˙ x^d =

¯f( x,¯

u,¯

¯0),

x¯^d(tk) =

¯xk (2.6)

where

¯x(t) =

x¯^d(t) +δ

x¯(t)andδ

x¯(t_k) =0, the process can then be written as

¯˙

x(t) = ˙

x¯^d+δx˙ (2.7a)

=

¯f( x¯^d+δ

¯x, u,¯

0¯ +

v¯) (2.7b)

=

¯f( x¯^d,

u,¯

0¯) +F( x¯^d,

u¯)δ x¯ +G

v¯ +. . . (2.7c) where the higher order moments are ignored.

The effects of the process noise over the interval ∆t can be approximated as

δ˙ x¯ =Fδ

x¯ +G

v¯ (2.8)

(17)

whereFand Gare the jacobians:

F= ^∂_¯^f

∂¯x^T

¯x^d, u¯

(2.9)

G = ^∂_¯^f

∂v¯

¯x^d, u¯

(2.10) and

δx¯(t_k)∼ N(

0, ˆ¯ P_k) (2.11)

The covariance att_k+1will then be

P¯_k+1 =Φ_kPˆ_kΦ^T_k +ΓQΓ^T (2.12) where

Φ˙ (t,t_k) = F(

¯x^d(t)_,

u¯(t))·Φ(t,t_k)_, Φ(t_k,t_k) = I (2.13) ΓQΓ^T =

t_k+∆t

Z

t_k

Φ(t,t_k)GQG˜ ^TΦ^T(t,t_k)dt (2.14)

To be able to simulate the stochastic differential equation

¯˙ x=

¯f( x,¯

u,¯ v¯);

¯x(tk) ∼_N(ˆ

x¯_k, ˆPk) (2.15) solve the equation

¯˙ x^d =

¯f(

¯x^d, u,¯

0¯)_,

x¯^d(t_k) = x¯_k δ˙

x¯ =F(

¯x^d, u¯)δ

x¯ +G( x¯^d,

u¯) v;¯ δ

x¯(t_k) ∼N( 0, ˆ¯ P_k)







t ∈[t_k,t_k+1] _(2.16)

ifu¯(t) =

u¯_kfort∈ [t_k,t_k+1]then x¯^d_k₊₁ =

¯f_k( x¯_k,

u¯_k), δ

x¯_k+1 =Γ_k

v¯_k (2.17)

¯x_k+1=

¯f_k(

¯x_k,

u¯_k) +Γ_k

v¯_k (2.18)

(18)

Discretization with Euler-integration and the first order in the Taylor series of the deterministic differential equation gives

¯˙ x^d=

¯f( x,¯

u,¯ 0¯),

¯x^d(t_k) =

x¯_k (2.19)

¯x^d_k₊₁=

¯x_k+∆·

¯f( x¯_k,

u¯_k) =

¯f_k(

¯x_k,

u¯_k) (2.20) IfF(

¯x^d(t),

u¯(t))in (2.13) is approximated as F(

x¯^d(t)_,

u¯(t))≈F(

¯x_k,

u¯_k) = F_k (2.21) then (2.14) can be discretizized as

Γ_kQ_kΓ_k^T =

t_k+1

Z

t_k

Φ(t,t_k)G( x¯^d,

u¯)Q^˜(t)G^T( x¯^d,

u¯)Φ^T(t,t_k)dt (2.22)

≈

t_k+1

Z

t_k

e^F^k⁽^t⁻^t^k⁾G( x¯_k,

u¯_k)Q^˜(t)G^T(

¯x_k,

u¯_k)(e^F^k⁽^t⁻^t^k⁾)^Tdt (2.23)

≈∆G( x¯_k,

u¯_k)QG^˜ ^T(

¯x_k,

u¯_k) (2.24)

If ˜Q = diag(σ₁²,σ₂²,· · ·σ_n²_v) = σ¯

σ¯^T then Γ_k = ∆¹^/²G

σ¯ and Qk = Invnv

The process can then be written on the discrete form x¯_k+1 =

x¯_k+∆·

¯f( x¯_k,

u¯_k) +Γ_k v¯_k =

¯f_k(

¯x_k,

u¯_k) +Γ_k

v¯_k (2.25) When ˜Qis diagonal

Γ_k =∆¹^/²G σ¯,

v¯_k ∼N(

0,¯ In_vn_v) _(2.26)

2.2 Extended Kalman Filter

The Extended Kalman Filter is a widely used estimator for non-linear state estimation, especially in navigation systems with GPS.

In an EKF the mean is propagated through the non-linear model, while the covariance is calculated through a linearization of the underlying non- linear model, the filter linearizes the non-linear function around the current estimate [8].

(19)

The EKF can be sensitive and may diverge quickly if the initial estimate, or the process model, is not accurate enough. It also seems to strug- gle with highly non-linear systems.

There are a number of modifications possible for the EKF to make it more suited for specific purposes, but in this thesis the focus is on the first order Extended Kalman Filter.

For a continuous - discrete system

¯˙ x =

¯f(

¯x,

u¯) +Gv (2.27a)

¯z_k = h¯(

¯z_k) +w_k (2.27b)

Predicted state

¯˙¯

x=

¯f(¯ x,¯

u¯), ¯

¯x(t_k) = ˆ

x¯_k (2.28)

The predicted covariance is calculated as with a regular Kalman Filter P˙¯ = FP¯+PF^¯ ^T+GQG˜ ^T (2.29) where he state transition matrix is defined as the jacobian

F= ^∂^f

∂x _ˆ

x,u

(2.30) Kalman gain

K_k = P^¯(t_k)H_k^T(HP¯(t_k)H_k^T+R)⁻¹ (2.31) Estimated covariance

Pˆk = (_I−_K_k_H_k)_P^¯_k _(2.32) where the observation matrix

H_k = ^∂h

∂x xˆ_k

(2.33) Estimated state

¯ˆ x_k = ¯

x¯ +K_k( z¯_k−

h¯_k(¯

x¯)) (2.34)

(20)

2.3 Unscented Kalman Filter

The Unscendet Kalman Filter (UKF) [7], is a filter used for non-linear systems. The UKF responds often better for highly non-linear system functions, than the EKF. This is because the UKF carries information regard- ing the mean and covariance through non-linear transformations, called unscented transformations. The unscented transform picks a set of sample points around the mean, these points are called sigma points. The next state and covariance is estimated by propagating the sigma points through the non-linear functions.

There are a number of different sets of sigma points which has been proposed [8]. There’s the symmetric set, the expanded symmetric set, and the scaled unscented transform. The symmetric set has an equal number of sigma points below and above the mean. The expanded set includes the mean as a sigma point, in addition to symmetric set. The scaled unscented transform builds on the expanded symmetric set, where there is defined one set of weight for the estimation of the mean, and another set of weights for the estimation of the covariance. The scaled unscented transform is more costomizable, and is thus more capable to reduce the effect of higher order moments in the non-linear functions.

Given the function

¯ y =

¯ g(

x¯) (2.35)

E{

x¯} = ¯

¯x (2.36)

Kov{

x¯} =P^¯ (2.37)

where

x¯ is a stochastic vector with known mean and covariance.

For an expanded set of symmetrical sigma points there are p sigma points.

p=2nx+1 (2.38)

wherenxis the number states.

(21)

If the covariance

P=UU^T; where U = [ u¯₁,

u¯₂, · · ·

u¯n_x] _(2.39) then the sigma points for the expanded symmetrical set can be calculated as

x¯⁰ = ¯

x¯ (2.40a)

x¯ⁱ = ¯ x¯ +

r nx

1−w⁰ui fori∈ {1, 2, . . . ,nx} (2.40b) x¯ⁱ⁺ⁿ^x = ¯

x¯ −

r nx

1−w⁰ui fori∈ {1, 2, . . . ,nx} _(2.40c) The choice of the first weight w⁰ decides how the sigma points are placed. For a w⁰ > 0 the points will be moved away from the mean ¯

x,¯ while aw⁰ <0 will move the points closer to ¯

x. A choice for the¯ w⁰for an expanded set could be

w⁰ =1−ⁿ^x

3 (2.41)

and the other weights can be defined as wⁱ = ¹−w⁰

2nx fori∈ {1, 2, . . . , 2nx} (2.42) The sigma points is then propagated through the non-linear function

¯

g(·)to get

¯ yⁱ =

¯ g(

x¯ⁱ) fori∈ {0, 1, . . . , 2nx} (2.43) The (2.43) can then be summed and weighted to get the predicted state

¯

¯ y=

2nx

∑

i=0

wⁱ

¯

yⁱ (2.44)

with the covariance P¯y =

2nx

∑

i=0

wⁱ( y¯ⁱ− ¯

¯y)(

¯yⁱ− ¯

y¯)^T (2.45)

The resulting estimates should carry through more information from the non-linear function. The UKF does require more calculational opera- tions than the EKF. But the UKF does not require the jacobian, it is therefore generally easier to implement.

(22)

3 Trajectory Generator and Calculation of Mea- surement and Input Data

The camera aided inertial navigation system uses a camera to assist the measurements from the on board accelerometer and gyroscope to improve its navigational estimates. The camera and INS needs to be modeled in such a way that they can be implemented and the system simulated. A trajectory generator also needs to be implemented to give a deterministic trajectory for the platform to follow. The known trajectory is used to cal- culate the input to the system from the accelerometer and gyroscope, and the measurements from the camera and/or GPS.

To be able to model the system accurately, noise must be added to the input from the gyroscope, accelerometer and the measurements given by the GPS and camera. The input to the system is modeled in the form u¯ = [^˜

¯f^b, ˜

ω¯ ^nb_b ]^T, and the measurement is modeled as the non-linear function with additive noise

z¯_k = h¯(

x¯_k) + w¯_k.

Figure 1 is a flow chart showing how the measurements from the GPS and camera

¯z_k, and the update from the INS

u, are connected with the tra-¯ jectory generator (TG).

The following sections gives a description of the trajectory generator, the inertial navigation system, the camera model and how the noise can be added to the measurement from the GPS and/or camera and the input data from the INS.

3.1 Trajectory Generator

A trajectory generator (TG) is needed to test and simulate the system. The TG should generate a known trajectory, based on a set of specified inputs, for the platform to follow. This will make it possible to run simulations on the generated trajectory, and compare the results with the known trajec-

(23)

TG x¯^d

Camera

GPS

INS error eq.

¯z_k

u¯ = [

¯f^b,

ω¯^nb_b ]^T+ [δ

¯f,δ ω¯]^T Stationary point(s)

w¯_k x¯^d,

¯f^b, ω¯^nb_b

[δ

¯f,δ ω¯]^T

Figure 1: Flow chart trajectory generator and sensor data to measurements z¯_kand input

u, with the measurement noise¯ w_kand the error equations from the INS.

tory. The input and measurement model will also use the known trajectory to generate input and measurement data for the filters.

One way of generating a trajectory is to specify the desired trajectory and orientation of the platform, and then derive the input

u. Another way¯ is the other way around, where the

¯f^b and

ω¯^nb_b are specified, and then integrated to the trajectory and orientation of the platform. The former method is used in this thesis.

If the desired trajectory is defined as

¯x^d = [

¯ pⁿ,

v¯ⁿ,

λ¯]^T _(3.1)

The dynamics of the TG can then be written as

¯˙ x^d =

¯f^tg(

¯x^d,

u¯^d) (3.2)

The implementation of the trajectory generator is described in section 5.1.

(24)

3.2 Inertial Navigation System

The Inertial Navigation System (INS) uses accelerometers and gyroscopes to estimate the motion and rotation of the platform [2]. The accelerometers measures the forces ˜

¯f^b acting on the platform, and the gyroscopes measures the angular velocity ˜

ω¯^nb_b of the platform. The input is modeled as

u¯ = [^˜

¯f^b, ˜

ω¯^nb_b ]^T = [

¯f^b,

ω¯^nb_b ]^T+ [δ

¯f^b,δ

ω¯^nb_b ]^T (3.3) whereδ

¯f^b andδ

ω¯^nb_b is the error equations for the input.

In the INS modeled in this thesis it is assumed that the earth is flat and non rotating. The system model for the position

¯pⁿ, velocity

v¯ⁿ, and orientation

λ¯ can be defined as

˙

¯ pⁿ =

v¯ⁿ (3.4)

¯˙

vⁿ = Rⁿ_b

¯f^b−

g¯ⁿ (3.5)

¯˙ λ =E(

λ¯)Rⁿ_b

ω¯^nb_b (3.6)

where Rⁿ_b is the rotation matrix for transforming a vector from the body frame to the inertial frame,

¯

gⁿ is the gravity acting on the platform, and ω¯^nb_b is the angular velocity of the platform. The subscriptsndenotes that the vector is seen from the inertial reference frame, while b denotes that the vector is seen from the body reference frame.

The orientation of the platform

λ¯ is represented in roll-pitch-yaw Euler angles

λ¯ =





 φ θ ψ







(3.7)

and the convertion matrixE(

λ¯)is here defined as E(

λ¯) =







cψ/cθ sψ/cθ 0

−sψ cψ 0 sθcψ/cθ sθsψ/cθ 1







(3.8)

(25)

where s is the sin function, and c is the cosine function.

The rotational matrixRⁿ_b for roll-pitch-yaw angles is here defined as

Rⁿ_b =







cψcθ −sψcφ+cψsθsφ sψsφ+cψsθcφ sψcθ cψcφ+sψsθsφ −cψsφ+sψsθcφ

−_sθ cθsφ cθcφ







(3.9)

3.3 Camera

The camera in this thesis is modeled as a normalized pinhole camera. The pinhole camera model is a simple way of modeling the camera without the effects from the lenses normally associated with a regular camera [5]. The pinhole camera can be described as a box, where the light from a scene passes through a pinhole aperture, into the light-proof box, and projects an inverted image on the opposite side of the box.

The geometry of the pinhole camera is quite simple, in such a way that straight lines can be drawn from each pixel in the image, to its correspond- ing point on the scene. In a normalized camera model, the focal length of the camera f is set to 1.

Figure 2 on the following page illustrates a point (or landmark) P projected on an image with the coordinates of the point in the camera reference frame

ρ¯^c, the pinhole image coordinatessx,sy, and focal length f. In actuality the image plane would be projected inverted after the focal point (the origin), but it is illustrated ahead of the focal point for simplicity, and to make it easier to visualize the geometry.

The relationship between the pinhole image coordinates

¯sand the coordinates for the landmark P seen in the cameras frame of reference

¯ ρ, as shown in figure 2, can be described as

sx

f = ^ρ

cx

ρ^c_z (3.10)

sy

f = ^ρ

cy

ρ^c_z (3.11)

(26)

Figure 2: Illustration of a point P projected on an image, for a pinhole camera, with the coordinates of the point in the camera reference frame

¯

ρ^c, the pinhole image coordinatess_x,s_y, and focal length f.

If sz is defined as sz = _ρ^fc z = _ρ¹c

z, where f = 1 in a normalized pinhole camera, the pinhole camera coordinates can then be expressed as

¯s =





 sx

sy

sz







= ¹ ρ^c_z





 ρ^c_x ρ^c_y 1







(3.12)

Figure 3 on the next page shows the geometry of a pointPwith reference to the inertial reference frame, and with reference to the camera and body reference frame, which here is modeled to coincide with each other. If the point P is modeled as stationary in the inertial reference frame ˙

¯rⁿ =0, then the dynamics of the pinhole image coordinates

¯scan be modeled as

(27)

Figure 3: Point P as a stationary point in the inertial reference frameFⁿ, seen from the inertial reference framen. The camera and body frame coincide. Where

¯ pⁿis the position of the platform,

¯rⁿis the position of the point P, and

¯

ρ^cis the position of the point P in the cameracand bodybreference frames.

in [6]

˙

sx =−v^c_xsz+sxv^c_zsz+sxsyω^c_x−(1+s²_x)ω^c_y+syω^c_z (3.13)

˙

sy=−v^c_ysz+syv^c_zsz+ (1+s²_x)ω^c_x−sxsyω^c_y−sxω^c_z (3.14)

˙

sz =v^c_zs²_z +szsyω^c_x−szsxω^c_y (3.15) where

v¯^cis the velocity in the cameras reference frame, andω^cis the angular velocity of the platform, thus connecting the camera coordinates with the motion and orientation of the platform.

3.4 Input and Measurement data

Noise components must be added to both the input

u¯ and the measurement ¯z_k. The input

u¯ consists of acceleration measurements

¯f^b from an

(28)

acceleromter, and angular velocity

ω¯^nb_b from a gyroscope. The measurements are modeled as

¯z_k =

"

¯z^cam_k

¯z^GPS_k

# +

"

w¯cam

w¯_GPS

# .

Input

The accelerometer is modeled with a white noise component

v¯a, and a biasb¯a associated with its measurement. The gyroscope is modeled with an instability in the bias

µ¯_g, in addition to a white noise component

v¯gand a bias

¯bg.

The white noises is modeled with a gaussian distribution, the bias is constant, and the instability for the bias in the gyroscope is modeled as a first order markov process. The error components in the accelerometer and gyroscope can thus be defined as

δ

¯f^b =

¯ba+

¯va (3.16)

δω¯ ^nb_b =

¯bg+ v¯g+

¯

µ_g (3.17)

where the white noise terms is defined as

v_k ∼ N(0,Q·δ_kl) (3.18) and the bias instability is defined as the first order markov process

¯˙

µg =−¹ Tµ¯_g+

v¯µ (3.19)

where Qis the covariance matrix for the noise,δ_kl is the kronecker delta, andTis a time constant.

Define the Power Spectral Density (PSD) for the system as

Q˜ =





 Q˜a

Q˜g

Q˜µ_g







(3.20)

(29)

where the PSDs for the accelerometer and gyroscope can be defined as Q˜a =

1

g10⁻⁶Da

2

(3.21) Q˜g =

2π 360Dg

2

(3.22) where Da and Dg is the noise densities for the accelerometer and gyroscope respectively. These densities are found in the manual [9], and the terms ahead of the densities converts them to the appropriate units.

If the continuous function of the system is on the form

¯˙

x(t) = F

x¯(t) +G

v¯(t) (3.23)

then the covariance of the process white noise can be described by the differential equation

P˙(t) = FP(t) +P(t)F^T+GQG˜ ^T (3.24) and the bias instability (3.19) can be described as

P˙µ_g(t) = −²

TPµ_g(t) +Q^˜µ_g (3.25) The steady state value of Pµ_g can be found by setting ˙Pµ_g(_t) = _{0, which} gives the PSD for the bias instability

Q˜µ_g = ²

TPµ_g(t) (3.26)

where

Pµ_g = 2π

360 · ¹ 3600

2

Sµ_g (3.27)

where Sµ_g is given in the manual [9], and the term ₃₆₀^2π · ₃₆₀₀¹ is to convert the noise from degrees/h, to rad/s.

It is then possible to approximate the discrete noise sample for simulations with the first order Euler method such that

Q^d =∆tQ,˜ Q^d =





 Q^d_a

Q^d_g Q_µ^d_g







(3.28)

(30)

where∆tis the sample interval.

Measurements

The GPS measurements is simply modeled as the true position from the known trajectory with a white noise term added.

¯z^GPS_k =

¯ pⁿ+

w¯GPS (3.29)

The camera measurements are modeled as the true pinhole coordinates calculated from the known trajectory, with a white noise term added. From figure 3 on page 15 it is seen that

¯rⁿ =

¯ pⁿ+

¯ ρ^c →

¯ ρ^c =

¯rⁿ−

¯

pⁿ (3.30)

The true pinhole coordinates (3.12) can then be defined as

¯s^d = ¹ ρ^c_z

"

ρ^c_x ρ^c_y

#

(3.31) The measurement function for the camera can therefore be defined as

¯z^cam_k =

¯s^d+

w¯cam (3.32)

with the white noise term

w¯cam. The camera is modeled such that the landmarks are visible as long as they are within range, independent on how the platform is rotated with respect to the landmarks.

The measurements can thus be defined by combining (3.29) and (3.32)

z¯_k =

"

¯z^cam_k

¯z_k^GPS

# +

"

w¯cam

w¯GPS

#

(3.33) The measurement function can be altered and expanded in accordance with how many landmarks the camera focuses on, and if one or both of the GPS/camera measurements is available.

(31)

4 System model on the standard form

4.1 System model

To be able to design and use the EKF and UKF filters the system needs to be on the standard form

¯˙ x =

¯f( x,¯

u,¯

¯v) (4.1)

¯z_k = h¯(

x¯_k,

w¯_k) (4.2)

for non-additive noise, or

¯˙ x =

¯f(

¯x,

u¯) +G

v¯ (4.3)

¯z_k = h¯(

x¯_k) +

w¯_k (4.4)

for additive noise [4].

While a deterministic trajectory generator gives

¯˙ x^d =

¯f^tg(

¯x^d,

u¯^d) (4.5)

where

x¯^d= [

¯ pⁿ,

v¯ⁿ, λ¯]^T

The input to the process is defined as

u¯ =

u¯^d+ [δ

¯f^b,δ

ω¯^nb_b ]^T (4.6)

where the error vectors for the accelerometer and gyroscope is defined as δ

¯f^b =

¯ba+

v¯a (4.7a)

δω¯^nb_b =

¯bg+ v¯g+

µ¯_g (4.7b)

Combining (4.6) with (4.5)

¯˙ x^d =

¯f^tg( x¯^d,

u¯ −[ b¯a+

v¯a,

¯bg+

¯vg+

¯

µ_g]^T) (4.8)

(32)

If a new state vector for the system is defined as

¯x= [

¯ pⁿ,

v¯ⁿ, λ,¯

b¯a, b¯g,

µ¯_g,s¹_x,s¹_y,s¹_z,· · · ,s_x^N,s^N_y,s^N_z ]^T (4.9) for aNlandmark system, where

¯

pⁿis the position in the inertial frame, v¯ⁿ is the velocity in the inertial frame,

λ¯ is the orientation of the platform in roll-pitch-yaw Euler angles,

¯ba and

b¯g is the biases for the accelerometer and gyroscope,

µ¯_g is the bias instability for the gyroscope, and lastly

¯s^N is the pinhole image coordinates for theNth landmark.

The state dynamics ˙

¯xfor a two landmark system can then be described as:

˙

¯pⁿ =

v¯ⁿ (4.10)

¯˙

vⁿ =Rⁿ_b

¯f^b−

¯

gⁿ (4.11)

¯˙

λ=E(λ)Rⁿ_b

ω¯^nb_b (4.12)

¯˙

b_a =0 (4.13)

˙

¯b_g=0 (4.14)

¯˙

µg =−¹ Tµ¯_g+

v¯µ (4.15)

˙

s¹_x =−v^c_xs¹_z+s¹_xv^c_zs¹_z+s¹_xs¹_yω^c_x−(1+ (s¹_x)²)ω^c_y+s¹_yω^c_z (4.16)

˙

s¹_y=−v^c_ys¹_z+s¹_yv^c_zs¹_z+ (₁+ (s¹_x)²)ω^c_x−s¹_xs¹_yω^c_y−s¹_xω^c_z (4.17)

˙

s¹_z =v^c_z(s¹_z)²+s¹_zs¹_yω^c_x−s¹_zs¹_xω^c_y (4.18)

˙

s²_x =−v^c_xs²_z+s²_xv^c_zs²_z+s²_xs²_yω^c_x−(1+ (s²_x)²)ω^c_y+s²_yω^c_z (4.19)

˙

s²_y=−v^c_ys²_z+s²_yv^c_zs²_z+ (1+ (s²_x)²)ω^c_x−s²_xs²_yω^c_y−s²_xω^c_z (4.20)

˙

s²_z =v^c_z(s²_z)²+s²_zs²_yω^c_x−s²_zs²_xω^c_y (4.21) where

v¯^c is the velocity in the camera frame, and ω¯^c =

ω¯ ^nb_b since the camera frame is defined to coincide with the body frame.

The equations can thus be written on the standard form for a non-linear state function with additive noise.

¯˙ x=

¯f( x,¯

u¯) +G

¯v (4.22)

which can be used to design the filters.

(33)

4.2 Error State Model

To implement the extended or unscented Kalman filter it is necessary to find the process noise covariance matrix Q_k. In [1], the process noise covariance matrix is proposed as

Q_k =∆t G(t_k)Q^ˇ(t_k)G^T(t_k) _(4.23) where ˇQ(t_k)is the power spectral density, and the noise matrix G(t_k)_, for a two landmark system, is given by

G(t_k) =







Gins(tk) G_cam¹ (t_k) G_cam² (t_k)







(4.24)

where the INS noise is

G_ins(t_k) =







0 0 0

Rˇⁿ_b 0 0 0 Rˇⁿ_b 0

0 0 0

0 0 I







(4.25)

and the camera noise

G¹_cam(t_k) =







0 sˇxsˇy −(1+sˇ²_x) sˇy 0 0 (1+sˇ²_y) −sˇxsˇy −sˇx 0 0 sˇzsˇy −sˇzsˇx 0 0







(4.26)

TheG²_cam(t_k)is identical in build to theG¹_cam(t_k)except that the normalized pinhole camera coordinates ˇsxand ˇsyare for the second landmark.

The process noise covariance presented is built on approximations, so it works best on systems where the sample period∆tis small compared to the rate of change in the system dynamics.

(34)

5 Program

This section describes the implementations of the trajectory generator, the input and sensor measurements, and the filters in matlab. The algorithms are mainly described with simplified pseudo code.

5.1 Trajectory Generator

The trajectory generator implemented uses the same principles as in [1].

Two types of movement are implemented, a straight line trajectory segment and a turning segment.

The straight trajectory segment generates a trajectory only in one direction, here defined as the x-direction, and with no rotational changes to the platform.

The turning segment is implemented such that it can turn in the x-y plane. The first half of the turn will follow the curvature of an Euler-spiral, and the second half will follow the same curvature but reversed. This will create smooth transitions in and out of the turn.

The output of the trajectory generator consists of the time t, position, velocity and acceleration in the inertial frame

¯ pⁿ,

v¯ⁿ,

¯aⁿ, acceleration in the body frame

¯a^b, the orientation in quaternions

θ¯, and the angular velocity ω¯^nb_b .

Straight

The input to the straight segment is the desired end velocityV[m/s], and the desired length L [m] over which the desired end velocity should be reached.

The total time is defined as

T = ^2L

V₀+V[s] (5.1)

whereV0is the start velocity.

(35)

The maximum acceleration for the desired velocity change over the total time

A= ^V−V₀

T (5.2)

A smooth acceleration is desirable, the acceleration in the inertial frame is therefore defined as

¯aⁿ(t) =







A(1−_cos(βt)) 0

0







(5.3)

where the constantβ= ^2π_T , and the rate of change to the acceleration (jerk) isAβsin(βt), .

The acceleration can then be integrated to the velocity, with start con- ditionV0

v¯ⁿ(t) =







V₀+A

t−^sin_β⁽^βt⁾ 0

0







(5.4)

Similarly the velocity can be integrated to the position, with start con- ditionP₀

¯

pⁿ(t) =







P0+V0t+A

t²

2 +^cos⁽_β^βt₂⁻¹⁾ 0

0







(5.5)

Since there are no rotation in the system for this segment, the rotation matrix Rⁿ_b(t), acceleration in the body frame

a¯^b(t) and angular velocity ω¯^b(t)is set to be

Rⁿ_b(t) = I (5.6)

¯a^b(t) = aⁿ(t) (5.7)

ω¯^b(t) = 0 (5.8)

(36)

By setting the end velocity equal to the start velocityV =V₀ the acceleration will be A = ^V⁻_T^V⁰ = 0, such that the trajectory will be a straight line with constant velocity, over the desired length L. This segment can easily be changed to accommodate movement in the other directions.

Turn

In order to have smooth changes in movement for the turn trajectories, Euler-spirals are used here. Half the turn is a part of a Euler-spiral, and for the other half the Euler-spiral is reversed, here called a reverse Euler- spiral. This enables the turn to both have a smooth transition into the turn, as well as out of the turn into a straight trajectory. The Euler-spiral has the property that the curvature changes linearly with its curve length.

The input to the turn segment is a desired turn radius r [m], and a desired turn angleθ_end [rad].

Time for half the turn, half in the Euler-spiral, and the other half in the reverse Euler-spiral.

T = ^2rθ^end 2V0

(5.9) whereV0is the start velocity.

Maximum acceleration in the turn A= ^V

02

r (5.10)

Firstly an Euler-spiral eases the platform into a turn, and generates the first half of the complete turn. The turn angel as a function of time, for the Euler-spiral is here defined as

θ_E(t) = ^At

2

2V0T (5.11)

The acceleration in the inertial frame as a function of time

¯aⁿ(t) = ^At T







−sin(θ_E(t)) cos(θ_E(t))

0







(5.12)

(37)

The velocity in the inertial frame as a function of time

v¯ⁿ(t) =V0







cos(θ_E(t)) sin(θ_E(t))

0







(5.13)

The position in the inertial frame as a function of time

¯pⁿ(t) = V0 t

Z

0







cos(θ_E(t)) sin(θ_E(t))

0







dτ (5.14)

The orientation as a function of time

Rⁿ_b(t) =







cos(θ_E(t)) −sin(θ_E(t)) 0 sin(θ_E(t)) cos(θ_E(t)) 0

0 0 1







(5.15)

The acceleration in the body frame as a function of time

¯a^b(t) = ^At T





 0 1 0







(5.16)

The angular velocity as a function of time

ω¯ ^nb_b (t) = ^At V₀T





 0 0 1







(5.17)

The Reverse Euler-spiral then reverses the turn, thus completing the turn by smoothly transition back into to a straight trajectory.

A new turn angle for the reverse Euler-spiral is defined as θ_RE(t) = ^A

V0

t− ^t

2

2T

(5.18) The calculations for the acceleration, velocity, position, orientation and angular velocity is done in the same way as for the regular Euler-spiral

(38)

(equations (5.12) to (5.17)) except that the time t is subtracted from the total timeT, resulting in the following equations

¯aⁿ(t) = ^A(T−t) T







−sin(θ_RE(t)) cos(θ_RE(_t))

0







(5.19)

v¯ⁿ(t) = V₀







cos(θ_RE(t)) sin(θ_RE(t))

0







(5.20)

¯

pⁿ(t) = V₀ Zt

0







cos(θ_RE(t)) sin(θ_RE(t))

0







dτ (5.21)

Rⁿ_b(t) =







cos(θ_RE(t)) −sin(θ_RE(t)) ₀ sin(θ_RE(t)) cos(θ_RE(t)) 0

0 0 1







(5.22)

¯a^b(t) = ^A(T−t) T





 0 1 0







(5.23)

ω¯^b(t) = ^A(T−t) V0T





 0 0 1







(5.24)

5.2 System Simulation

The system dynamics is simulated using the discrete time state transition function via the Euler’s Method

¯x_k+1=

¯x_k+∆t·

¯f_k(

¯x_k,

u¯_k) _(5.25)

where

u¯k = [

¯ f˜b,

¯˜

ω^nb_b ], and the step size ∆tis defined as 1 over the prediction frequency.

Vision aided INS

Vision aided INS

Comparing the UKF and EKF

Ivan Flatval

Thesis submitted for the degree of Master in Science in Electrical Engineering

30 credits

Department of Physics

Faculty of mathematics and natural sciences

UNIVERSITY OF OSLO

Vision aided INS

Comparing the UKF and EKF

Ivan Flatval

Abstract

Preface

Contents

List of Figures

List of Algorithms

1 Introduction

2 Extended and Unscented Kalman Filter

2.1 Discretization of Non-Linear Stochastic Processes

2.2 Extended Kalman Filter

2.3 Unscented Kalman Filter

∑

∑

3 Trajectory Generator and Calculation of Mea- surement and Input Data

3.1 Trajectory Generator

3.2 Inertial Navigation System

3.3 Camera

3.4 Input and Measurement data

4 System model on the standard form

4.1 System model

4.2 Error State Model

5 Program

5.1 Trajectory Generator

5.2 System Simulation