9 vues

Transféré par sadiksnm

rob

- Anticolission System
- ISI_MStat_08
- 2012-1807. Kinematics Robot Manipulators
- s1-ln1738690224394924-1939656818Hwf-2140726153IdV138225312517386902PDF_HI0001
- Probability Cheatsheet
- 99_062.pdf
- Calculation and Identification of the Aerodynamic Parameters
- Science Direct Exchange Rate
- 3208 Probabilistic Matrix Factorization
- SPE-175111-MS.pdf
- Direction Cosine Matrix
- [1] Transfer Learning a Riemannian Geometry
- Toby Lai Thesis
- NITI-Aayog for SSC and Bank
- Scheme Pm1
- 1st Week Insight (4th Jan to 10th Jan) of 2016
- 1st Week Main Events (4th Jan to 10th Jan)
- 11 AP History Practice Bits
- ANSYS CFX STUDENT USER MANUAL
- Ch2-Components of food-NCERT classVI

Vous êtes sur la page 1sur 11

Vijay Kumar

University of Pennsylvania

The Kalman filter is a tractable instance of the Bayes filter and allows us to compute an estimate of the state vector

x and its distribution at time t from estimate at time t − 1. In order to make it tractable, we must make three

assumptions:

1. The process model, p(xt |ut , xt−1 ), is linear with additive noise. In other words,

xt = Axt−1 + Bt ut−1 + nt (1)

where A is a nonsingular n × n matrix, B is a n × m matrix, u is a m × 1 vector of inputs that is applied

between time t − 1 and t, the process noise nt is Gaussian white noise1 with zero mean and covariance Q. We

denote this by nt ∼ N (0, Q).

2. The measurement model, p(zt |xt ), is linear with additive noise. In other words,

zt = Cxt + vt (2)

where zt is a p × 1 vector, C is a p × n matrix, and the measurement noise vt is Gaussian white noise with

zero mean and covariance R, or vt ∼ N (0, R).

3. The prior on the state x is p(x0 ) is normally distributed with a known mean µ0 and covariance Σ0 , or p(x0 ) ∼

N (µ0 , Σ0 ).

The filter consists of two steps, the prediction step that relates the predicted state, xt̄ at time t̄ (think of t̄ as a

time instant just before t) to the estimated state xt−1 at time t − 1, and the update step that computes the estimated

state xt at time t.

Definition 1 Multivariate normal distribution

Let X be a vector of random variables. fX (x) : Rn → R is a multivariate normal distribution (Gaussian) if it

takes the form:

1 1 T −1

fX (x) = n√ exp − (x − µ) Σ (x − µ) ,

(2π) 2 det Σ 2

where µ ∈ Rn and Σ ∈ Rn×n is a n × n, symmetric positive definite matrix.

1

White noise is a signal whose samples are a sequence of serially independent random variables, with each sample having a normal

distribution with zero mean.

R X is fX (x) as defined

If the density function of above, X is said to be multivariate normal. You should be able

to verify2 that fX (x) ≥ 0, Rn fX (x)dx = 1, Rn xfX (x)dx = µ, and Σ = E((X − µ)((X − µ)T ).

R

X1

X= ,

X2

X1 and X2 are both (multivariate) Gaussians and are jointly normally distributed.

X1

X=

X2

X1

X=

X2

is a normal distribution, X1 and X2 are both normally distributed.

Fact 1 If

X1

X=

X2

is a multivariate Gaussian and X1 and X2 are uncorrelated, i.e., the covariance matrix has the form:

Σ11 Σ12

Σ= , Σ12 = Σ21 = 0,

Σ21 Σ22

Remark 2 If X1 and X2 are independent, they are also uncorrelated. The converse of Fact 1 does not require the

Gaussian assumption. Independent random variables are always uncorrelated.

Fact 2 The sum of independent Gaussian random variables is also Gaussian. Let X, Y be two independent Gaus-

sian random variables with means µX , µY and covariances ΣX , ΣY , respectively. Then their sum, Z = X + Y , is

also Gaussian with mean µZ = µX + µY and covariance ΣZ = ΣX + ΣY .

Fact 3 Affine transformations of Gaussian distributions are Gaussian. If X is a n-dimensional multivariate Gaus-

sian with the mean µX and covariance ΣX , and if Y = AX + b where A is a n × n matrix and b ∈ Rn , then Y is a

multivariate Gaussian with mean µY and covariance ΣY given by:

µY = AµX + b, ΣY = AΣX AT .

The next two results may not be obvious but are well known. See [1] for example. A particularly nice introduc-

tion for the uninitiated is provided in [5].

2

See Wikipedia for the evaluation of the Gaussian integral.

2

Fact 4 Conditional density of a multivariate normal distribution. If

X1

X=

X2

is a multivariate Gaussian, with the means µ1 and µ2 for the vectors X1 and X2 and the covariance

Σ11 Σ12

Σ= ,

Σ21 Σ22

the conditional density fX1 |X2 (x1 |X2 = x2 ) is a multivariate normal distribution for a given x2 with the mean and

covariance

µX1 |X2 = µ1 + Σ12 Σ−1 −1

22 (x2 − µ2 ), ΣX1 |X2 = Σ11 − Σ12 Σ22 Σ21 .

Fact 5 X1 − µX1 |X2 and X2 are independent. To prove this first show that X1 − µX1 |X2 and X2 are joint normal

(the vector is a affine transformations of X = [X1 , X2 ]) and show the covariance is zero.

An overview of the two steps and the final equations are provided in the schematic in Figure 1.

xt 1 xt = Axt 1 + Bt ut + nt

(µt 1 , ⌃t 1 )

recursion

State at time t̄

State at time t

xt̄

xt µ̄t = Aµ̄t 1 + But

¯ t = A⌃t 1 AT + Q

⌃

µt = µ̄t + Kt (zt Ct̄ µ̄t )

⌃t = ⌃¯ t Kt C ⌃¯t

Update

zt = Cxt + vt

Prediction Step This step computes the predicted state, xt̄ (and its distribution) at time t̄ (think of t̄ as a time

instant just before t, but t̄ and t have the same value) from the estimated state xt−1 (and its distribution) at time t − 1.

Because the process model in (1) is an affine transformation of the prior state and the sum of independent Gaussian

random variables (since the noise is independent of the initial state) from Fact 3 and Fact 2,

µ̄t = Aµ̄t−1 + But (3)

3

Σ̄t = AΣt−1 AT + Q (4)

Update Step The update step computes the estimated state xt (and its distribution) at time t from the prediction at

time t̄ in (3,4). We known that without the measurement zt , the best we can do is write xt = xt̄ . But if we combine

this with (2) we can write

xt = xt̄

zt = Cxt + vt

to get

xt I 0 xt̄

= (5)

zt C I vt

x

From Fact 3, we know that t is joint normal with the mean given by

zt

µ̄t

C µ̄t

Σ̄t 0 I C T Σ̄t C T

I 0 Σ̄t

= .

C I 0 R 0 I C Σ̄t C Σ̄t C T + R

We are interested in the conditional distribution of xt given the measurement zt . From Fact 4, we know that

p(x1 |X = x2 ) is normal. Therefore p(xt |Zt = zt ) is normal and the conditional mean and covariance are given by:

If we denote by Kt the Kalman gain,

Kt = Σ̄t C T (C Σ̄t C T + R)−1 ,

we can write (without denoting explicitly the fact that the expectation and covariance are conditioned on the mea-

surement):

µt = µ̄t + Kt (zt − C µ̄t ) (8)

Σt = Σ̄t − Kt C Σ̄t . (9)

Thus the Kalman filter is given by Equations (3, 4, 8, 9).

The Kalman gain effectively represents a tradeoff between the prediction and the update. If the confidence in the

prediction is high relative to the measurement (Q is small, R is large), Kt is small and the distribution at time t will

be more strongly influenced by the prediction.

Remark 3 If the distribution is not Gaussian but the white noise is uncorrelated with x0 , the Kalman filter represents

the minimum variance linear estimator. In other words, if we want a linear estimator of xt , Equations (3-9) represent

the best estimator. See [1].

4

4 The Extended Kalman Filter

If the system is nonlinear, the methodology of the previous section can be applied with a linearized model of the

system, and updating this model at every time step by linearizing about the current state.

Let the continuous time process model be given by a nonlinear, stochastic differential equation:

ẋ = f (x, u, n) (10)

where x and u are the state and input respectively, and n is Gaussian white noise, n(t) ∼ N (0, Q). The observation

model is given by:

z = h(x, v) (11)

where z is the measurement vector and v is Gaussian white noise, v(t) ∼ N (0, R).

As before, we consider a time interval, (t0 , t̄), where t0 is the same as the previous time step t − 1. The prediction

phase requires us to integrate the process model over this time interval δt = t̄ − t0 :

This integration may be difficult to do analytically. However, any numerical integration scheme can be used to

perform this integration. For example, a simple one-step Euler integration yields the update:

Alternatively, one can linearize this system about the most likely previous pose, i.e., the mean, (x = µt−1 , u =

ut , n = 0) to write:

∂f ∂f ∂f

f (x, u, n) ≈ f (µt−1 , ut , 0) + (x − µt−1 ) + (u − ut ) + (n − 0)

∂x (µt−1 ,ut ,0) ∂u (µt−1 ,ut ,0) ∂n (µt−1 ,ut ,0)

| {z } | {z } | {z }

At Bt Ut

= f (µt−1 , ut , 0) + At (x − µt−1 ) + Bt (u − ut ) + Ut (n − 0) (14)

and explicitly integrate this matrix differential equation over the time interval (t0 , t) to get Φ(t; xt−1 , u, n). We can

also use the linear model to approximate the next pose using (13) and (14)

:0

≈ xt−1 + f (µt−1 , ut , 0) + At (xt−1 − µt−1 ) + Bt t−

(u u t ) + Vt (nt − 0) δt

= (I + At δt) xt−1 + (Ut δt) nt + f (µt−1 , ut , 0) − At µt−1 δt

| {z } | {z } | {z }

Ft Vt bt

= Ft xt−1 + bt + Vt nt (15)

Using the linearized models from (13)–(15) and Facts 2–3, we are able to find the predicted mean and covariance

of the state. The mean is

µ̄t = Ft µt−1 + bt

= (I + At δt)µt−1 + f (µt−1 , ut , 0) − At µt−1 ) δt

= µt−1 + f (µt−1 , ut , 0) δt, (16)

5

and the covariance is given by

Σ̄t = Ft Σt−1 FtT + Vt Qt VtT . (17)

For the update phase, we similarly linearize the observation model z = h(x, v) around the most likely predicted

state (x = µ̄t , v = 0) to be able to use the results of the previous section and write:

∂h ∂h

zt ≈ h(µ̄t , 0) + (x − µ̄t ) + (v − 0)

∂x (µ̄t ,0) ∂v (µ̄t ,0)

| {z } | {z }

Ct Wt

= h(µ̄t , 0) + Ct (x − µ̄t ) + Wt (v − 0) (18)

The Kalman Filter was based on the linear model in (1) and (2). We are now in a position to write the equations

for the Extended Kalman Filter using either (12) or (14) instead of (1), and (18) instead of (2).

Prediction Step We compute the predicted state, xt̄ (and its distribution) at time t̄ from the estimated state xt−1

(and its distribution) at time t − 1. The expected state, µ̄t , is obtained either by integration over the time interval

(t − 1, t̄):

µ̄t = Φ(t̄; µt−1 , ut , 0) (19)

or from the linear approximation at time t − 1:

Either way we can invoke the affine process model in (20) to find the covariance using Fact 3:

For the update step we use the linear approximation using the state µ̄t at time t̄ to write:

xt I 0 xt̄ 0

= + (23)

zt Ct W t v t h(µ̄t , 0) − Ct µ̄t

where

Kt = Σ̄t CtT (Ct Σ̄t CtT + Wt Rt WtT )−1 .

6

5 Application to Quadrotors

In our approach we do not explicitly model the inputs (the propellor speeds or the motor currents) since often these

inputs can be quite different from the commanded inputs and difficult to measure in small flying robots. We instead

define our state so that their derivatives can be written in terms of sensed quantities obtained from the Inertial

Measurement Unit (IMU). Said another way, we will abstract out the dynamics so we can write the model in terms

of kinematic quantities.

There are two approaches to doing this. First, we can assume that we have a very good velocity sensor that

gives you excellent estimates of the linear velocity of the vehicle. This allows us to eliminate the velocity (and the

acceleration) from the state vector as we will see below in Section 5.1. Alternatively, we can relax this assumption

and instead use the acceleration information obtained from the IMU. See Fig. 2. This forces us to retain the velocity

in the state vector but eliminate the motor torques and the propellor speeds from the process model. This is explained

in Section 5.2.

Figure 2: A sample data set from the IMU used in the lab. The measured accelerations (left) and the angular velocity

(right) are modeled by a bias and an additive noise.

Process Model We write the state vector, x ∈ R9 , as:

x1 p

x = x2 = q

x3 bg

7

where p is the position, q is a 3 × 1 vector parametrization of SO(3), and bg is the gyro bias. In what follows we use

the Euler angle parameterization (despite its limitations when the rotation is significantly different from the identity

element):

φ

q = θ ,

ψ

where ψ is the yaw angle, φ is the roll angle, and θ is the pitch following the Z − X − Y Euler angle notation. Recall

that the rotation matrix is given by

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

R(q) = cθsψ + cψsφsθ cφcψ sψsθ − cψcθsφ .

−cφsθ sφ cφcθ

and the components of the angular velocity in the body frame (aligned with the axes of the gyro) are given by:

p cθ 0 −cφsθ φ̇

q = 0 1 sφ θ̇

r sθ 0 cφcθ ψ̇

ω = G(q)q̇ (26)

We assume we have good velocity measurements vm in the inertial frame from the V ICON motion capture

camera system. The gyro measurements ωm are in the body-fixed frame and have a bias bg that drifts over time. For

the gyro measurements we write:

ωm = ω + bg + ng (27)

where ng is an additive measurement noise modeled by a Gaussian, white noise. The velocity measurement vm is

given by:

vm = ṗ + nv (28)

where nv is an additive Gaussian, white noise. We assume that the drift in the bias vector is described by Gaussian,

white noise random processes:

ḃg = nbg (t) (29)

where nbg ∼ N (0, Qg ).

Without explicitly modeling the inputs (the propellor speeds or the motor currents), we develop the process

model from Equations (26-35) as follows:

vm − nv

ẋ = G(x2 )−1 (ωm − bg − ng ) . (30)

nbg

8

Measurement model We will use a camera to measure the pose of the robot in an inertial frame. We use the

theory of projective transformations and solutions of the 2D-3D pose discussed in the computer vision segment of

the class. Specifically we saw how to estimate the pose of the robot using a minimum of four features (e.g., April

Tags) on the ground plane. In other words, given the projection equations for four features (i = 1, . . . , 4):

Xi

ui Yi

vi ∼ K [R(q) p]

3×4 0 ,

w

W

or

ui Xi

vi = H3×3 Yi , H ∼ K R̄(q) p

3×3

1 1

where R̄ is formed by deleting the third column of the rotation matrix. With four features we can estimate H up to

scale, and with a known K we can recover R by solving for K −1 H, constructing H 0 by replacing the last column

of K −1 H with the cross product of the first two rows, and then finding R:

R = arg min kR − H 0 k2 .

SO(3)

Thus our measurement model is linear because we can obtain the rotation matrix R, and therefore the vector q

as well as the position p:

p

z= = Cx (31)

q

where C is a 6 × 9 matrix:

I3×3 0 0

C=

0 I3×3 0

Extended Kalman Filter Equations (30) and (31) should be sufficient to write the EKF. Note that (30) is nonlinear

and the linearization of the equations might require the use of a symbolic manipulator like Mathematica or Matlab.

Process Model In this model, we assume that we cannot directly measure velocity but can measure acceleration

using an accelerometer. Accordingly, the state vector, x ∈ R15 is written as:

x1 p

x2 q

x= x3 = ṗ

x4 bg

x5 ba

where p is the position and ṗ is the velocity of the center of mass, q is a 3 × 1 vector parametrization of SO(3), bg

is the gyro bias, and ba is the accelerometer bias. As in Section 5.1, we use the Euler angle parameterization and

write the components of the angular velocity in the body frame which are aligned with the axes of the accelerometer

9

and the gyro. The gyro measurements ωm and the accelerometer measurements am are in the body-fixed frame and

have biases bg and ba that drift over time. For the gyro measurements we write:

ωm = ω + b g + n g (32)

where ng is an additive measurement noise modeled by a Gaussian, white noise. The accelerometer measurement

am is given by:

am = R(q)T (p̈ − g) + ba + na (33)

where g is the acceleration due to gravity in the inertial frame (the [0, 0, 1]T vector), and na is an additive Gaussian,

white noise. We assume that the drift in the bias vectors are described by Gaussian, white noise random processes:

ḃg = nbg (t) (34)

ḃa = nba (t) (35)

where nbg ∼ N (0, Qg ) and nba ∼ N (0, Qa ).

Without explicitly modeling the inputs (the propellor speeds or the motor currents), we develop the process

model from Equations (26-35) as follows:

x3

G(x2 )−1 (ωm − bg − ng )

g + R(q)(am − ba − na ) .

ẋ = (36)

nbg

nba

Measurement model As in Section 5.1, we use a camera to measure the pose of the robot in an inertial frame. We

can also use the onboard camera to estimate the velocity of the robot from optical flow. Alternatively the V ICON

camera measurements can also be used to provide a measurement of the velocity.

Thus our measurement model is linear because we can obtain the rotation matrix R, and therefore the vector q

as well as the position p and velocity ṗ:

p

z = q = Cx (37)

ṗ

where C is a 9 × 15 matrix:

I3×3 0 0 0 0

C= 0 I3×3 0 0 0

0 0 I3×3 0 0

Extended Kalman Filter Equations (36) and (37) should be sufficient to write the EKF. Note that (36) is nonlinear

and the linearization of the equations might require the use of a symbolic manipulator like Mathematica or Matlab.

5.3 Extensions

There are two simplifying assumptions in this treatment. First, we have used the Euler angle parameterization in our

state. It would have been better to use either quaternions or exponential coordinates in the derivation. Second, we

have assumed that the camera is a sensor that yields the pose as a measurement. In reality, the camera is a noisy

sensor and the tracking of the features is also a noisy process. Thus the estimation of the pose should include a

model of this noise. A more rigorous treatment of the Vision-aided Inertial Navigation (VIN) problem that addresses

both these limitations is available in [4].

10

References

[1] B. D. O. Anderson and J. B. Moore, Optimal Filtering, Dover Publications, 2005.

[4] D. G. Kottas and J. A. Hesch and S. L. Bowman and S. I. Roumeliotis, On the Consistency of Vision-aided

Inertial Navigation, International Symposium on Experimental Robotics, Quebec, Canada, June 2012.

[5] M. Mintz, Notes on the Multivariate Normal Distribution and the Discrete-Time Kalman Filter, University of

Pennsylvania, 2006.

11

- Anticolission SystemTransféré parchezz43
- ISI_MStat_08Transféré parapi-26401608
- 2012-1807. Kinematics Robot ManipulatorsTransféré parSiddhartha Mishra
- s1-ln1738690224394924-1939656818Hwf-2140726153IdV138225312517386902PDF_HI0001Transféré parAnonymous 2h5lIe
- Probability CheatsheetTransféré parMoaaz Mahdi
- 99_062.pdfTransféré parRăzvan Popa
- Calculation and Identification of the Aerodynamic ParametersTransféré pargustavo12
- Science Direct Exchange RateTransféré parsomeone08
- 3208 Probabilistic Matrix FactorizationTransféré parAhmed1972
- SPE-175111-MS.pdfTransféré parmejiasid
- Direction Cosine MatrixTransféré parsazrad
- [1] Transfer Learning a Riemannian GeometryTransféré parchristianfv5

- Toby Lai ThesisTransféré parsadiksnm
- NITI-Aayog for SSC and BankTransféré parShan Reddy
- Scheme Pm1Transféré parmanpreetsingh3458417
- 1st Week Insight (4th Jan to 10th Jan) of 2016Transféré parsadiksnm
- 1st Week Main Events (4th Jan to 10th Jan)Transféré parsadiksnm
- 11 AP History Practice BitsTransféré parvijaigk
- ANSYS CFX STUDENT USER MANUALTransféré parvdnsit
- Ch2-Components of food-NCERT classVITransféré parHussain
- syllabusTransféré parsadiksnm
- Dinamica Newton EulerTransféré parPaul Alvarez Herrera
- IES CONV Mechanical Engineering 1985Transféré parcoolpawan10
- Chapter 5Transféré paranil.gelra5140
- Current-Affairs-2016-Telugu-Bit-Bank-Download-4.pdfTransféré parsadiksnm
- Informacion!!Transféré parCristian Alejandro Reyes Juarez
- 1st Week Q&A (4th Jan to 10th Jan)Transféré parsadiksnm
- Sample QuestionsTransféré parsadiksnm
- Chapter 1Transféré paralexandraanastasia
- OutputTransféré parsadiksnm
- Lecture 45Transféré parsadiksnm
- ptTransféré parsadiksnm
- AfmTransféré parsadiksnm
- Plm 20dec17Transféré parsadiksnm
- 2006Transféré parsadiksnm
- kinematics of machineTransféré parDevendra Meena
- 2017_2Transféré parsadiksnm
- 1_draft.pdfTransféré parHung Nguyen Huy
- Sample Questions.pdfTransféré parsadiksnm
- 37023454 Tower FoundationTransféré parkurtcobain12
- 2006.pdfTransféré parsadiksnm

- How to Start Preparation for IIT JEETransféré parshekhar5893
- A Review Of the Ancient Concepts of Chinese MedicineTransféré parGerald D. Friz
- What is Truelift?Transféré parTruelift
- Collaborative Mentoring HandbookTransféré parCarol Harington
- Power Systems Program Conceptual ModelTransféré paryared4
- classroom observation assignment-form 1 abdulkadir ciplakTransféré parapi-322522017
- MFL Level DescriptorsTransféré parJai Tragik
- How to Do Research at the MIT AI LabTransféré parillusionperpetuelle
- Cosleeping- A Universal Human PhenomenonTransféré parFotini Vittou
- MECH Multiple Choice 1984Transféré parclarencetan95
- Sharia and Tariqa: Islamic Law and Purification of the HeartTransféré parHassan Shibly
- Information and Communication Technologies (ICT)Transféré parTuhinBiswas
- final summative strategiesTransféré parapi-295321202
- A 264418Transféré parkispatkany
- primary_prog.pdfTransféré parKogillavany Vany
- PROVA 2º Ano 2º TRITransféré parCamila Veilchen Oliveira
- Women in Mining: A Guide to Integrating Women into the WorkforceTransféré parIFC Sustainability
- cogni_ans_1Transféré parsaif127
- Lucid Paper v2Transféré parVu Dang Tung
- Milroy&Milroy-Linguist Change Social Network and Speaker InnovationTransféré parSidhantha Jain
- chan2019.pdfTransféré parNorberth Ioan Okros
- articol 1Transféré parannasescu
- Cai - Probability Matching Tolerance Intervals for Distributions in Exponential FamiliesTransféré parambrofos
- Goldilocks and the Three Bears ES1Transféré parS TANCRED
- t Svar IntroTransféré parTrinh Zit
- H.P. Blavatsky Collected Writings - VOLUME XIVTransféré parjeanjaures24
- The Power of PurposeTransféré parsilntrunin
- 2. Millerite Adventism Through 1844Transféré parJulienne Rowelie Aujero Sanchez
- The Ontario Readers: The High School Reader, 1886 by Ontario. Ministry of EducationTransféré parGutenberg.org
- Chem Sem 2 Unit 1 LAB PenniesTransféré parCharles Cao