0 views

Uploaded by yunierv

- visual motion by conditional denstiy_thesis
- A Survey of Sensorless Initial Rotor Position Estimation Shc
- 4309-12955-1-PB
- Discrete Kalman filter
- kf
- MAF_05
- web.txt
- SLAM (Localización y Mapeo Simultaneo)
- Ramon Garcia-Hernandez, Michel Lopez-Franco, Edgar N. Sanchez, Alma y. Alanis, Jose a. Ruz-Hernandez Auth. Decentralized Neural Control Application to Robotics
- Gps 9 Kalman Filtering
- Unabridged collation about multifarious computing methods and outreaching cloud computing based on innovative procedure
- Decentralized Estimation and Control for Multisensor Systems
- Kalman Filter Explained
- Kalman Intro
- 10.1109@ivs.2004.1336492
- This is a Comprehensive Guide for All Those Students Who Are Appearing for Board Exams This Year
- ncctm presentation 2016
- curriculum connection december 4
- Perovic Dordevic Paskota Takaci Jovanovic 40
- letter to parents

You are on page 1of 7

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π ;

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)

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

− 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

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

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

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

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

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

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.

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

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)

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.

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)

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)

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.

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

2

I 0 0 0 σω̄

zk = xk + (4.28)

0 0 I 0 σr̄2

| {z } | {za }

H Rk

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

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

- visual motion by conditional denstiy_thesisUploaded byapi-3855364
- A Survey of Sensorless Initial Rotor Position Estimation ShcUploaded by조용규
- 4309-12955-1-PBUploaded byirfan
- Discrete Kalman filterUploaded byTrần Việt Phương
- kfUploaded byAnggra Novilia
- MAF_05Uploaded byRazvan
- web.txtUploaded bydraculaclone
- SLAM (Localización y Mapeo Simultaneo)Uploaded byMario Olivares
- Ramon Garcia-Hernandez, Michel Lopez-Franco, Edgar N. Sanchez, Alma y. Alanis, Jose a. Ruz-Hernandez Auth. Decentralized Neural Control Application to RoboticsUploaded byIvan Hernandez
- Gps 9 Kalman FilteringUploaded bysbgayen
- Unabridged collation about multifarious computing methods and outreaching cloud computing based on innovative procedureUploaded byJournal of Computing
- Decentralized Estimation and Control for Multisensor SystemsUploaded byCeebBoumerdes
- Kalman Filter ExplainedUploaded bymohgawa
- Kalman IntroUploaded bylanhtuchudu
- 10.1109@ivs.2004.1336492Uploaded byRider Paredes Maraza
- This is a Comprehensive Guide for All Those Students Who Are Appearing for Board Exams This YearUploaded byShreshtha Tiwari
- ncctm presentation 2016Uploaded byapi-350931537
- curriculum connection december 4Uploaded byapi-239641222
- Perovic Dordevic Paskota Takaci Jovanovic 40Uploaded byMariano Leonel Acosta
- letter to parentsUploaded byapi-253812240
- Rocket Pattern T GraphUploaded bytlavoie2
- P4 Math CA1 2014 Worksheet - Word Problems Pt 1Uploaded byGuan Jie Khoo
- GLOBK_Ref_10.3Uploaded byaypsh
- Luis Tan-AOK Module 1 (Part 2)Uploaded byLuis Antone Tan
- mathunit 1Uploaded byapi-208843281
- Ryan Go Lei SurveyUploaded byryanlei24
- School MathematicsUploaded bynick
- Bibliography Teaching for DepthUploaded byrafiqcu
- Results Reference Gas AnalysersUploaded byl0k0tus
- tws 3 short range learning objectivesUploaded byapi-176512276

- 1017_ifaccep96Uploaded byyunierv
- Discr IdentUploaded byyunierv
- Filter kalman vs complementaryUploaded byDante Fajardo Olmos
- APM2_5BlockDiagramUploaded byyunierv
- APM2_5BlockDiagramUploaded byyunierv
- APM2_5BlockDiagramUploaded byyunierv
- Real-Time Simulation of Ship Motions in WavesUploaded byyunierv
- GNC ToolboxUploaded byyunierv
- Submarine Dynamic ModelingUploaded bykooec
- 2015_07_BreguTesis Reengineering APM.pdfUploaded byyunierv
- Documento Sobre BarcosUploaded byyunierv
- artigo_backstepping2_4.pdfUploaded byyunierv
- APM2_5BlockDiagramUploaded byyunierv
- APM2_5BlockDiagramUploaded byyunierv
- Articulo OK VerUploaded byyunierv
- Design of a Sliding Mode Fuzzy Controller for AUVUploaded byRuchit Pathak
- Time-varying non-linear system identification using dynamical adaptive fuzzy systemsUploaded byyunierv
- CONFERENCIA1Uploaded byyunierv
- 96-376-3-PBUploaded byyunierv
- Informática 2013. Esquema de Control en Espacio de Tarea Para Simuladores de Movimiento de Dos y Tres Grados de Libertad.Uploaded byyunierv
- 39646bUploaded byJulian Amaya
- Diurnal.pdfUploaded byyunierv
- Introduccion_vida_devota San Fransisco SalesUploaded byMatias
- Nonlinear Path Following with Applications to the Control of Autonomous Underwater VehiclesUploaded byyunierv
- OCW-CCE S19 Respuesta en Frecuencia de Amplificadores Con TransistoresUploaded byyunierv
- Correction of Bathymetric Survey Artifacts Resulting Apparent Wave-Induced Vertical Position of an AUVUploaded byyunierv
- Apuntes sobre las estrategias de control para el seguimiento de camino de un veh´ıculo aut´onomo subacu´atico.Uploaded byyunierv
- Fossen MCMC12 (Perturbaciones) Descarg 2013Uploaded byyunierv
- Practicas Electronica II PspiceUploaded byManuel A. Ríos M

- 20_NT_IUploaded byMohammed Eljammal
- 2.1 Rates of ChangeUploaded byRenee Edwards McKnight
- IdealsUploaded byMihnea
- Effect of water-in-diesel emulsion on performance and emission characteristics of DI diesel engine: A ReviewUploaded byIJAMTES
- Coleman - Vacuum decay .pdfUploaded byGuillermo Gerardo Rivera Gambini
- R05420105-PRESTRESSEDCONCRETEUploaded byvanamgoutham
- MMC 4609 - Communication Research Strategy - Exam 2 ReviewUploaded byMark Fiorentino
- RH2000 EnglishUploaded bykaniappan sakthivel
- 21-DC - HWDP - NM - FlexUploaded bynjileo
- Beam-searching and Transmission Scheduling in.pdfUploaded byMohammad
- CCTV SpecUploaded byJeffrey Teo
- Sargent T., Et. Al (2010) - Practicing DynareUploaded byMiguel Ataurima Arellano
- maths literature review elloise updated adUploaded byapi-316881659
- Lab Programs GUIUploaded byDVADONE
- datasheetTA8207K.pdfUploaded byvitry
- rao don DA p 29, 37Uploaded byhoanglanhou
- 7N90Uploaded byttnaing
- EMD Important Questions Unit IV - Conventional Speed ControlsUploaded byJoseph Harindranath
- AP Invoice Payments AllUploaded byrahulpardeshi2005
- Quick Start Guide en SystemRescueCdUploaded byalmagata
- blog-COTS_HALT-HASS.pdfUploaded bysadeq
- Force VectorsUploaded byparkl
- An14 Installing the Auto LockUploaded bymoorthysanmukam
- 87zxcUploaded byjovmicic
- Information Fusion for Intelligence AnalysisUploaded byDavid R Vidal
- Data Interpretation for IBPS 2018Uploaded byRakesh
- Glass Lining TechUploaded bytecvidya
- A_Comparison_of_VLF_and_50Hz_Field_Testing_of_MV_Cables.pdfUploaded byGualadrake
- OT - Simulation Problem Set 2Uploaded byIIDT BA
- Leica Na2 Nak2Uploaded bybbutros_317684077