Chapter 4

State Estimation

Navigation of an unmanned vehicle, always depends on a good estimation of the vehicle states. Espe-
cially if no external sensors or markers are available, more or less complex algorithms have to be used.
For this project, the orientation estimation is done by the widely used Kalman filter. However, position
estimation is still an object of ongoing research. The here described approach uses the company own
software R ECONSTRUCT M E.

4.1 Orientation
With using Euler angles (Z − Y ′ − X ′′ ) between −π and π, it should also be taken care of potential
problems referring to the transition between maxima. For this project, Φ and Θ do not need special
treatment, as the robot is not expected to exceed the maxima, since this would mean that the thrust vector
is pointing to the ground. For the same reason the singularity at Θ = 90◦ of the used Euler angle
representation of the orientation is irrelevant. However, the Ψ angle can and will make this mentioned
maxima transition. This demands special treatment considering the calculation of e in the PID controller
in Fig. 3.2b. To compensate this overflow, a simple algorithm can be used:

1 i f ( eΨ > π )
2 Ψ −= 2π ;
3 e l s e i f ( eΨ < −π )
4 Ψ += 2π ;

4.1.1 Kalman Filter

A good introduction to the topic can be found in [52]. A more detailed description of the concept,
derivation and properties is given in [50]. The Kalman filter, also known as linear quadratic estimator, is
an optimal estimator for discrete linear system of the form

xk+1 = Ak xk + wk (4.1)

with the state xk ∈ Rn , and a measurement

z k = Hk x k + v k (4.2)

with zk ∈ Rm , if process noise wk and the measurement noise vk are assumed to be independent random
variables with Gaussian probability density functions and zero mean value. The normal probability
distributions p are then
2 , σ2 , . . . }
p(w) ∼ N (0, Q) , with Q = diag{σw1 w2 (4.3)
2 , σ2 , . . . }
p(v) ∼ N (0, R) , with R = diag{σv1 v2

with σ 2 being the variance of the corresponding noise distribution. The n×n matix Ak in (4.1) describes
the transition of the system between time step k and k + 1, in the absence of neither input nor process

27
4 State Estimation 4.1.1 Kalman Filter 28

noise. The m × n matrix H in the measurement equation (4.2) relates the state xk to the measurement zk .
The filter (in one popular algebraic form) consists of two stages. First, a prediction of the state x̂−
k at time
k, using the system dynamic matrix Ak−1 and state x̂k−1 in (4.1) from the last iteration k − 1. This will
be referred to as a priori estimation1 . Second, a correction of this estimation, using the measurements
zk . Using the a priori estimation x̂−
k in (4.2), an innovation variable (or residual) yk can be defined as

yk = zk − Hk x̂−
k. (4.4)

The innovation variable is used to correct the a priori prediction using the n × m gain matrix Kk

x̂k = x̂−
k + Kyk . (4.5)

This estimation is referred to as a posteriori estimation x̂k . The gain matrix Kk describes a blending
factor between the model based prediction, and the measurement. To achieve a readable description of
the optimal choice for Kk , the a priori and a posteriori estimate errors are defined as

e−
k ≡ xk − x̂−
k (4.6)
ek ≡ xk − x̂k

and the corresponding error covariances are

 − T

P−
k = E (xk − x̂−
k )(xk − x̂k )  (4.7)
Pk = E (xk − x̂k )(xk − x̂k )T .

The core task of the Kalman filter, is to choose the gain matrix Kk so that the a posterioiri estimate error
covariance Pk is minimized with respect of the mean square error. For more details on that derivation
see [50]. One popular form of the resulting Kk that minimizes the a posteriori estimate error covariance
is the so called Kalman gain
T − T

−1
K k = P−
k Hk Hk P k Hk + R k . (4.8)

It can be seen, that the covariance Rk of the measurement noise defined in (4.3) and the a priori error
covariance P− k defined in (4.7) have direct influence on the optimal gain matrix Kk . To point out the
extreme cases, if Rk is zero meaning that the measurement is very trustworthy, the gain matrix becomes

lim Kk = H−1
k (4.9)
Rk →0

and thus (4.5) degenerates to

x̂k = H−1 zk . (4.10)
The estimation then is only based on the measurement. On the other hand, if the error covariance P−
k
approaches zero, meaning that the model based estimation is accurate, the gain matrix Kk does not
consider the measurement at all
lim Kk =0
P−k →0 (4.11)
x̂k (K = 0) = x̂−
k.
The summary of the whole Kalman filter algorithm is shown in Fig. 4.1.
1
note the super script − in x̂−
k and later in Pk
−
4 State Estimation 4.1.2 Extended Kalman Filter 29

Initial estimates for x̂k and Pk

a posteriori correction
a priori prediction (1) Compute the Kalman gain
T H P− HT + R −1


(1) Project the state ahead K k = P− k H k k k k k
x̂−
k+1 = Ak x̂k (2) Update estimate with measurement
zk
(2) Project the error covariance ahead x̂k = x̂−
k + K k z k − H x̂
k k
−

P− T
k+1 = Ak Pk Ak + Qk (3) Update the error covariance
Pk = (I − Kk Hk ) P− k

Figure 4.1: Data flow of the Kalman filter operation

4.1.2 Extended Kalman Filter

For estimation of a non-linear system, several extended versions of the Kalman filter exist. A widely
used approach is to linearize the system dynamics in every step around the a priori estimation x̂k , and
proceed as for a linear system. This approach is known as the EKF. The fundamental imperfection of
the EKF, as also pointed out in [52], is that the distributions of the various random variables are no
longer normal, after undergoing their respective non-linear transformations. Thus, the optimality of the
estimation is only approximated by linearization. The stochastic system equations from (4.1) and (4.2)
are now generalized to the non-linear case

xk+1 = f (xk , wk ) (4.12)

again with the state vector xk ∈ Rn and

zk = h(xk , vk ) (4.13)

with the measurement vector zk ∈ Rm . By deriving the Jacobian matrix of the partial derivatives of f
and h with respect to the state vector xk and the noise vectors wk and vk
  
∂f (xk , wk ) T 
Alin,k =  (4.14a)
∂xk  −
x̂k

 T 
∂f (xk , wk ) 
Wlin,k =  (4.14b)
∂wk 
x̂−
k
 T 
∂h(xk , vk ) 
Hlin,k =  (4.14c)
∂xk 
x̂−
k
 T 
∂h(xk , vk ) 
Vlin,k =  , (4.14d)
∂vk 
x̂−
k

the EKF is implemented as shown in Fig. 4.2. A detailed derivation can again be found in [52] and [50].
4 State Estimation 4.1.3 Orientation Estimation in Use 30

Initial estimates for x̂k and Pk

a priori prediction
(1) Project the state ahead
x̂−
k+1 = f (x̂k , 0)
(2) Project the error covariance ahead
P− T
k+1 = Alin,k Pk Alin,k + Wlin,k Qk Wlin,k

a posteriori correction
(1) Compute the Kalman gain
 −1
K k = P− H T H P − T
H + V R V
k lin,k lin,k k lin,k lin,k k lin,k
(2) Update estimate with measurement
zk
x̂k = x̂−
k + K k z k − h(x̂ −
k , 0)
(3) Update the error covariance
Pk = (I − Kk Hlin,k ) P− k

Figure 4.2: Data flow of the extended Kalman filter operation

4.1.3 Orientation Estimation in Use

The orientation estimator in use, as already implemented in the open-source code [18] in the module
attitude_estimator_ekf, is an EKF. As there did not exist a documentation of the module, the code
was analyzed and is described in this section. The state vector and measurement vector are
 
B ω IB  
 B ω̇ IB  B ω̄ IB
x= 
 B rg  , z =
 B r̄g  (4.15)
r̄
B m
B rm

with the angular velocity of the quadcopter B ω IB = [ω x , ω y , ω z ]T , the estimated angular acceleration
T T
B ω̇ IB = [ω̇ x , ω̇ y , ω̇ z ] , the vector of the earth’s gravitation field B rg = [B rg,x ,B rg,y ,B rg,z ] and
T
the magnetic field vector B rm = [B rm,x ,B rm,y ,B rm,z ] . The available measurements are the angular
velocities B ω̄ IB from the gyroscopic sensors, the vector of gravitation B r̄g from the acceleration sensors2
and the vector of the magnetic field of the earth B r̄m directly from the magnetic sensors on the IMU. Note
that all variables are described in the body fixed coordinate system (compare chapter 2.2.1). To simplify
notation, the left side index B will be omitted in the following explanation and if no other coordinate
index is given, the variable shall be referred with respect to the body fixed system.

4.1.3.1 Prediction Step

Under the assumption of the angular acceleration staying constant during the time step ∆t, the angular
velocity B ω IB for the next time k + 1 can be predicted with ω k+1 = ω k + ω̇ k ∆t. The prediction of the
two vector fields can be derived similarly with rk+1 = rk + vk ∆t. However, the velocity vectors B va,k
and B vm,k are not directly measured. They can be derived from the relation of velocity and position in
2
neglecting the actual translational accelerations of the quadcopter, as they are assumed to be smaller by factor 10
4 State Estimation 4.1.3 Orientation Estimation in Use 31

the inertial frame

d
Iv = Ir (4.16)
dt
with
Bv = ABI d I r
dt
= ABI d (AIB B r)
dt
= ABI AIB B ṙ + ABI ȦIB B r (4.17)
| {z } | {z }
I e IB
Bω
= B ṙ +Bω e IB B r.
As for the gravitation and magnetic vector field the translational velocities B ṙ are irrelevant, because the
acting force is invariant to translational movement, only the rotational terms B ω e IB B r have to be taken
into account. Thus, the non-linear system dynamics are described with
 
ω k + ω̇ k ∆t + wω ,k
 ω̇ k + wω̇ ,k 
x− = f (x k , w) =  . (4.18)
k+1  ra,k + ωe k ra,k ∆t + wra,k 
rm,k + ωe k rm,k ∆t + wrm,k

For the two equations needed for the algorithm shown in Fig. 4.2, the matrices Alin,k and Wlin,k , as
shown in (4.14a) and (4.14b), are the calculated Jacobi matrices3
 
 I I∆t 0 0
∂f (xk , wk )  
 0 I 0 0 

Alin,k = =
 −  −e  (4.19a)
∂xk x̂k
ra,k ∆t 0 I+ω e k ∆t 0
−e
rm,k ∆t 0 0 I+ω e k ∆t
 
 I 0 0 0
∂f (xk , wk )   0 I 0 0 
Wlin,k =  =
 0
. (4.19b)
∂wk x̂−
0 I 0 
k
0 0 0 I
The implemented formulas for the a priori prediction step at time k + 1 are

x̂−
k+1 = f (x̂k , 0) (4.20)

with neglected process noise w, and

T
P−
k+1 = Alin,k Pk Alin,k + Qk (4.21)

where the measurement noise matrix Qk holds the variances σ 2 of the states as diagonal entries. They
represent the uncertainty of the prediction. As this variances are unknown and can not be measured, they
act as tuning variables for the filter.

4.1.3.2 Correction Step

Unlike the state prediction, the measurement of the system can be described with trivial linear functions
   2 
I 0 0 0 σω̄

zk = 0 0 I 0 x k +   σr̄2a  . (4.22)
0 0 0 I σr̄2m
| {z } | {z }
H Rk

3
ab = −e
Note the cross product is anticommutative: e ba in line 3 and 4 of matrix definition Alin,k
4 State Estimation 4.1.3 Orientation Estimation in Use 32

Thus, the linear observation matrix H is already given, and also the Jacobian matrix from (4.14d), lin-
earizing the influence of the measurement noise R, degenerates to a identity matrix I. The implemented
functions for the correction step as listed in Fig. 4.2 are directly given with the Kalman gain
T − T

−1
K k = P−
k Hk Hk P k Hk + R k , (4.23)

the a posteriori estimation

x̂k = x̂− −
k + Kk zk − Hk x̂k (4.24)
and the propagation of the error covariance during the correction step

Pk = (I − Kk Hk ) P−
k. (4.25)

For the first iteration, initial values for the first a posteriori have to be chosen. The first estimation x̂k is
initialized with measurements
   
B ω IB B ω̄ IB
 B ω̇ IB   0 
x̂k,init = 
 B ra 
 =
 B r̄g 
 . (4.26)

B rm k,init B r̄m k,init

The first error covariance is initialized4 with Pk,init = 100I. Over time, the chosen initial value of the
error covariance matrix does not have any influence, as it will converge, as long as it is not initialized
with equal to 0.

4.1.3.3 Incomplete measurements

As the three 3D sensors (gyro, acceleration, magnetometer) deliver measurements at independent sample
rates, the measurement vector from (4.22) will not always be fully available. To avoid using old mea-
surements multiple times, the observation matrix Hk and noise matrix Rk vary in their size, depending
on which sensor has new measurements available. If all sensors have new measurements, the vectors are
as in (4.22). As the gyroscope sensor has the fastest sampling rate, and is the most crucial measurement
for the estimation of the angular acceleration ω̇, the correction step will only be calculated, once a new
ω̄ measurement is available. For an incomplete measurement, the matrices are given with
  2


zk = I 0 0 0 xk + σω̄ (4.27)
| {z } | {z }
H Rk

if only gyroscope measurements are available,

   2 
I 0 0 0 σω̄
zk = xk + (4.28)
0 0 I 0 σr̄2
| {z } | {za }
H Rk

for gyroscope and acceleration measurements and

   2 
I 0 0 0 σω̄
zk = xk + . (4.29)
0 0 0 I σr̄2m
| {z } | {z }
H Rk

in case of gyroscope and a magnetometer measurements. The remaining formalism (4.23) to (4.25)
remains the same. Subsequently, only the states for which measurements are available will be corrected.
4
I is the identity matrix
4 State Estimation 4.2 Position 33

4.1.3.4 Extraction of the Orientation

In every iteration step of the Kalman filter, the angular velocity B ω IB and acceleration B ω̇ IB are esti-
mated. The orientation which is also from interest has to be extracted. To avoid any singularity problems,
the orientation is directly represented with the 3 × 3 rotation matrix ABI by describing the three unit
base vectors of the initial coordinate system in the body fixed coordinate system
 
ABI = B eIx B eIy B eIz . (4.30)

The three base vectors are extracted from the estimated acceleration vector B r̂a and the estimated mag-
netic field vector B r̂m . Note, that a NED inertial coordinate system is used. As mentioned above, by
neglecting the translational acceleration of the robot over the gravity field, the base vector in z-direction
is given by5
B ra
B eIz = − . (4.31)
kB ra k2
This estimation is especially accurate if the copter is hovering. Furthermore, the inertial system can
always be chosen in a way that the base vector in x direction corresponds with the magnetic field of
the earth. However, as the estimation will never be accurate, this vector is not directly taken to derive
B rIx . Rather to preserve orthogonality of the inertial system, it is used to calculate the base vector in
y-direction
B eI z × B r m
B eI y = . (4.32)
kB eIz × B rm k2
Given two base vectors, the third one can always be calculated for an orthogonal system:

B eI y × B eI z
B eI x = . (4.33)
kB eIy × B eIz k2

4.2 Position
Initial experiments were done using the PX4FLOW module [6]. It is a flow sensor module based on
low-cost components. See chapter 5.3.3 for a more detailed description of the module.To gather veloc-
ity information in x- and y direction, an optical flow estimation is done based on the sum of absolute
differences (SAD) block matching algorithm with angular rate compensation using the gyroscope mea-
surement [33]. The sensor module further delivers height measurement using a sonar sensor.
However, the results of using the PX4FLOW as a standalone position estimation by integrating the mea-
surements, were not very satisfying due to an unacceptable drift. For now, the position estimation process
is only based on the software R ECONSTRUCT M E [37]. Unlike common SLAM algorihms such as Davi-
son’s MonoSLAM [30] or Klein and Murray’s PTAM [36], the here used approach does create real
surfaces instead of than only dense maps.

4.2.1 ReconstructMe
The software R ECONSTRUCT M E that is used for position estimation, is developed by the company
P ROFACTOR in Steyr (Austria). It is a portable, low-cost 3D body scanning system [37]. It works with
publicly available low-cost active technology, such as the here used P RIME S ENSE C ARMINE 1.09 [5].
The algorithms are processed on decent standard computer hardware, which is designed for accelerated
processing (e.g. general purpose computation on graphics processing unit (GPGPU)). The sensor uses
the approach of structured light for depth calculation [35]. To do so, a constant pattern of speckles is
projected onto the scene by splitting up a laser beam via a diffraction grid. This pattern is captured by a
rP
5
To provide the normalized unit base vectors, the 2-Norm kvk2 = vi2 is used.
i