Applications in Industry - University of Queenslandrobotics.itee.uq.edu.au/~elec3004/2016/lectures/L22-Applications.pdf · “Review Lab” (“Lab 5”): • Redo any aspects of

1

Applications in Industry – (Extended) Kalman Filter

© 2016 School of Information Technology and Electrical Engineering at The University of Queensland

TexPoint fonts used in EMF.

Read the TexPoint manual before you delete this box.: AAAAA

http://elec3004.com

Lecture Schedule: Week Date Lecture Title

29-Feb Introduction

3-Mar Systems Overview

7-Mar Systems as Maps & Signals as Vectors

10-Mar Data Acquisition & Sampling

14-Mar Sampling Theory

17-Mar Antialiasing Filters

21-Mar Discrete System Analysis

24-Mar Convolution Review

28-Mar

31-Mar

4-Apr Frequency Response & Filter Analysis

7-Apr Filters

11-Apr Digital Filters

14-Apr Digital Filters

18-Apr Digital Windows

21-Apr FFT

25-Apr Holiday

28-Apr Introduction to Feedback Control

3-May Holiday

5-May Feedback Control & Regulation

9-May Servoregulation/PID

12-May Introduction to (Digital) Control

16-May Digital Control Design & State-Space

19-May Observability, Controllability & Stability of Digital Systems

23-MayDigital Control Systems: Shaping the

Dynamic Response & Estimation26-May Applications in Industry

30-May System Identification & Information Theory

2-Jun Summary and Course Review

Holiday

1

13

7

8

9

10

11

12

6

2

3

4

5

26 May 2016 - ELEC 3004: Systems 2

http://itee.uq.edu.au/~metr4202/

http://creativecommons.org/licenses/by-nc-sa/3.0/au/deed.en_US

http://elec3004.com/

http://elec3004.com/

2

Announcements

PS 3 Peer Review Competition

• The PS 3 Review with the

highest Likert Score

• Deadline for reviews:

June 3 (11:59 pm)

• Good reviews discussed

June 2nd Last Lecture

• Reward: 3004¢

ELEC 3004

“Review Lab”

(“Lab 5”):

• Redo any aspects of any

of the labs

• Review course

• Review 2015 Final exam

(which we will do

on June 2nd also)

!


G. Franklin,

J. Powell,

M. Workman

Digital Control

of Dynamic Systems

1990

TJ216.F72 1990

[Available as

UQ Ebook]

Follow Along Reading:

B. P. Lathi

Signal processing

and linear systems

1998

TK5102.9.L38 1998

State-space

• FPW

– Chapter 6 - Design of Digital

Control Systems Using State-Space

Methods

• Lathi Ch. 2 (?)

– § 2.7-6 Time Constant and Rate of Information Transmission

• Information Theory!

Today


https://library.uq.edu.au/record=b1604253~S7

http://robotics.itee.uq.edu.au/~elec3004/tutes.html#FPW





http://library.uq.edu.au/record=b2013253~S7

3

• Nothing beyond this point is on the exam.

(except for the exam review )

• Do not pay attention.

• Do not attempt to learn.

</assessable>

WARNING: Not assessable


Example 0:


4

TF 2 SS – Control Canonical Form)


Control Canonical Form as a Block Diagram


5

• CCF is not the only way to tf2ss

• Partial-fraction expansion of the system

System poles appear as diagonals of Am

• Two issues: – The elements of matrix maybe complex if the poles are complex

– It is non-diagonal with repeated poles

Modal Form


Modal Form


6

Modal Form Block Diagram


• Given: 𝑌 𝑠

𝑈 𝑠=

25.04𝑠+5.008

𝑠3+5.03247𝑠2+25.1026𝑠+5.008

Get a state space representation of this system

• Matlab:

• Answer:

Matlab’s tf2ss


7

Example 1:

Inverted Pendulum


Digital Control

Wikipedia,

Cart and pole


8

Inverted Pendulum


• The equations of motion of an inverted pendulum (under a

small angle approximation) may be linearized as:

𝜃 = 𝜔

𝜔 = 𝜃 = 𝑄2𝜃 + 𝑃𝑢

Where:

𝑄2 =𝑀 +𝑚

𝑀𝑙𝑔

𝑃 =1

𝑀𝑙.

If we further assume unity Ml (𝑀𝑙 ≈ 1), then 𝑃 ≈ 1

Inverted Pendulum – Equations of Motion


9

• We then select a state-vector as:

𝒙 =𝜃𝜔

, hence 𝒙 = 𝜃

𝜔 =

𝜔𝜔

• Hence giving a state-space model as:

𝐴 =0 1𝑄2 0

,𝐵 =01

• The resolvent of which is:

Φ 𝑠 = 𝑠𝐼 − 𝐴 −1 =𝑠 −1

−𝑄2 𝑠

−1

=1

𝑠2 − 𝑄2

𝑠 1𝑄2 𝑠

• And a state-transition matrix as:

Φ 𝑡 =cosh 𝑄𝑡

sinh 𝑄𝑡

𝑄𝑄 𝑠𝑖𝑛ℎ 𝑄𝑡 cosh𝑄𝑡

Inverted Pendulum –State Space


Cart & Pole in State-Space With Obstacles?

Swing-up is a little more than stabilization…

See also: METR4202 – Tutorial 11: http://robotics.itee.uq.edu.au/~metr4202/tpl/t11-Week11-pendulum.pdf


http://robotics.itee.uq.edu.au/~metr4202/tpl/t11-Week11-pendulum.pdf






10

Cart & Pole in State-Space Swing-up is a little more than stabilization…


Example 2:

Command Shaping


11

Experiments: Scanning Over Obstacle


Command Shaping


12

Robust Control: Command Shaping for Vibration Reduction

Command

ShappingRegulator

Integrated

Planner

Controller

Error PlantΣ+

–

SensorTunning


Command Shaping

Original velocity profile

Input shaper

Command-shaped velocity profile

Ve

locity

Ve

locity

Time

Time

Time*


13

Command Shaping

• Zero Vibration (ZV)

• Zero Vibration and Derivative (ZVD)

20

11

1

di

i

TK

K

Kt

A

21

eK

ddi

i

TT

K

K

K

K

Kt

A

20

)1()1(

2

)1(

12

2

22


Experiments: Command Shaping


14

Estimation:

Yet another way

to beat the noise


Along multiple dimensions


15

State Space • We collect our set of uncertain variables into a vector …

x = [x1, x2,…, xN]T

• The set of values that x might take on is termed the state space

• There is a single true value for x,

but it is unknown


State Space Dynamics


16

Measured versus True • Measurement errors are inevitable

• So, add Noise to State... – State Dynamics becomes:

• Can represent this as a “Normal” Distribution


Recovering The Truth • Numerous methods

• Termed “Estimation” because we are trying to estimate the

truth from the signal

• A strategy discovered by Gauss

• Least Squares in Matrix Representation


17

Recovering the Truth: Terminology


General Problem…


18

Duals and Dual Terminology


Estimation Process in Pictures


19

Kalman Filter Process


KF Process in Equations


20

KF Considerations


Ex: Kinematic KF: Tracking • Consider a System with Constant Acceleration


21

In Summary

• KF: – The true state (x) is separate from the measured (z)

– Lets you combine prior controls knowledge with

measurements to filter signals and find the truth

– It regulates the covariance (P) • As P is the scatter between z and x

• So, if P 0, then z x (measurements truth)

• EKF: – Takes a Taylor series approximation to get a local “F” (and

“G” and “H”)


Estimation:

“Bayesian Perspective”


22

• (Optimal) estimation of the (hidden) state of a

linear dynamic process of which we obtain noisy (partial)

measurements

• Example: radar tracking of an airplane.

What is the state of an airplane given noisy radar

measurements of the airplane’s position?

Kalman Filtering

ELEC 3004: Systems 26 May 2016 - 43

Model • Discrete time steps, continuous state-space

• (Hidden) state: xt , measurement: yt

• Airplane example:

• Position, speed and acceleration

tt

t

t

t

t x

x

x

x~,

yx


23

Dynamics and Observation model • Linear dynamics model describes relation between the state

and the next state, and the observation:

• Airplane example (if process has time-step ):

),(~

),(~

,

,1

RNV

QNW

C

A

tt

tt

tt

tt

t

t

0v

0w

vx

wx

y

x

001,

100

10

1 2

21

CA


• Let X0 be a normal distribution

of the initial state x0

• Then, every Xt is a normal distribution

of hidden state xt. Recursive definition:

• And every Yt is a normal distribution of observation yt.

Definition:

• Goal of filtering: compute conditional distribution

Normal distributions


ttt WAXX 1

ttt VCXY

ttt YYX yy ,,| 00

24

• Because Xt’s and Yt’s are normal

distributions,

is also a normal distribution

• Normal distribution is fully

specified by mean and covariance

• We denote:

Problem reduces to computing xt|t and Pt|t

Normal distribution


stst

sstsst

sstst

PN

YYXYYXN

YYXX

||

0000

00|

,ˆ

,,|Var,,,|E

,,|

x

yyyy

yy

ttt YYX yy ,,| 00

• Kalman filtering algorithm: repeat… – Time update:

from Xt|t, compute a priori distribution Xt+1|t

– Measurement update:

from Xt+1|t (and given yt+1), compute

a posteriori distribution Xt+1|t+1

Recursive update of state


X0 X1 X2 X3 X4 X5

Y1 Y2 Y3 Y4 Y5

…

25

Time update • From Xt|t, compute a priori distribution Xt+1|t:

• So:

QAAPAN

WAXAWXAN

WAXWAXN

WAXX

T

tttt

t

T

ttttt

tttttt

ttttt

||

||

||

||1

,ˆ

VarVar,EE

Var,E

x

QAAPP

A

T

tttt

tttt

||1

||1ˆˆ xx


From Xt+1|t (and given yt+1), compute Xt+1|t+1.

1. Compute a priori distribution of the observation

Yt+1|t from Xt+1|t:

Measurement update


RCCPCN

VCXCVXCN

VCXVCXN

VCXY

T

tttt

t

T

ttttt

tttttt

ttttt

|1|1

1|11|1

1|11|1

1|1|1

,ˆ

VarVar,EE

Var,E

x

26

2. Look at joint distribution of Xt+1|t and Yt+1|t:

where

Measurement update (cont’d)


RCCPCP

CPP

CN

YXY

YXX

Y

XNYX

T

tttt

T

tttt

tt

tt

tttttt

tttttt

tt

tt

tttt

|1|1

|1|1

|1

|1

|1|1|1

|1|1|1

|1

|1

|1|1

,ˆ

ˆ

Var,Cov

,CovVar,

E

E,

x

x

tt

tt

ttttttt

tttttttt

CP

XC

XVXXC

XVCXXY

|1

|1

|11|1|1

|11|1|11

Var

,Cov,Cov

,Cov,Cov

• Recall that if

then

3. Compute Xt+1|t +1 = (Xt+1|t|Yt+1|t = yt+1):

Measurement update (cont’d)


2221

1211

2

1

21 ,,

NZZ

21

1

22121122

1

22121221 ,| zz NZZ

tt

T

tt

T

tttt

ttt

T

tt

T

tttt

ttttttt

CPRCCPCPP

CRCCPCPN

YXX

|1

1

|1|1|1

|11

1

|1|1|1

1|1|11|1

,ˆˆ

|

xyx

y

27

This can also (often) be written in terms of the

Kalman gain matrix:

Measurement update (cont’d):


ttttttt

tttttttt

T

tt

T

ttt

CPKPP

CK

RCCPCPK

|11|11|1

|111|11|1

1

|1|11

ˆˆˆ

xyxx

• Choose distribution of initial state by picking x0 and P0

• Start with measurement update given measurement y0

• Choice for Q and R (identity) – small Q: dynamics “trusted” more

– small R: measurements “trusted” more

Initialization


28

I. Model:

II. Algorithm: Repeat… – Time update:

– Measurement update:

(Bayesian) Kalman Filter Summary


QAAPP

A

T

tttt

tttt

||1

||1ˆˆ xx

ttttttt

tttttttt

T

tt

T

ttt

CPKPP

CK

RCCPCPK

|11|11|1

|111|11|1

1

|1|11

ˆˆˆ

xyxx

),(~

),(~

,

,1

RNV

QNW

C

A

tt

tt

tt

tt

t

t

0v

0w

vx

wx

y

x

Take Aways:

• Kalman filter can be used in real time

• Use xt|t’s as optimal estimate of state at time t, and use Pt|t as a

measure of uncertainty.

Extensions:

• Dynamic process with known control input

• Non-linear dynamic process

• Kalman smoothing: compute optimal estimate of state xt

given all data y1, …, yT,

with T > t (not real-time).

• Automatic parameter (Q and R) fitting using EM-algorithm

(Bayesian) Kalman Filter Summary [II]


29

• Information Theory & More!

• Review: – Chapter 6 of FPW

– Chapter 13 of Lathi

• Deeper Pondering??

Next Time…


Final Exam Review

• June 10, 2016

• From: 2-4

• In: 45-204 (???)

• Some Review Notes

(from Course Textbooks) http://robotics.itee.uq.edu.au/~elec3004/tutes.html


http://robotics.itee.uq.edu.au/~elec3004/2014/lectures/SomeReviewNotes.ELEC3004.pdf






http://robotics.itee.uq.edu.au/~elec3004/tutes.html




Documents

Applications in Industry - University of Queenslandrobotics.itee.uq.edu.au/~elec3004/2016/lectures/L22-Applications.pdf · “Review Lab” (“Lab 5”): • Redo any aspects of