ME597: AUTONOMOUS MOBILE ROBOTICSSECTION 2 – LINEAR SYSTEMS

Prof. Steven Waslander

Scalar, Vector, Matrix

Fat matrix: n<m, Skinny matrix: n>m

Unit Vector, Identity Matrix

2

LINEAR ALGEBRA

11 12

21n m

nm

A AA A

A

1

2 n

n

bb

b

b

c

0

1ie

1 00 1I

Matrix Transpose

Matrix Addition

Matrix Multiplication

3

LINEAR ALGEBRA

11 11 12 12

21 21

A B A BA B A B

1 1 1 2

2 1

i i i ii i

i ii

A B A B

AB A B

11 21

12T

A AA A

Matrix Transpose of Added Matrices

Matrix Transpose of Multiplied Matrices

Quadratic form

4

LINEAR ALGEBRA

( )T T TAB B A

( )T T TA B A B

( ) ( )2

T T T T T T T

T T T T T

T T

Ax b Ax b x A Ax x A b b Ax b bx A Ax x A b b bx Cx d x e

Quadratic term

Matrix Rank: The number of independent rows or columns Nonsingular = Full Rank

Singular = Not full rank

Non-empty nullspace

Matrix Inverse (square A)

Nonsingular and square <=> Invertible5

LINEAR ALGEBRA

( )A

( ) min( , )A n m

( ) min( , )A n m

such that 0x Ax

1 1AA A A I

Matrix Trace

Symmetric Matrix

Positive Definiteness (Semi-Definiteness) For a symmetric nXn matrix A, and for any x in Rn

6

LINEAR ALGEBRA

( ) iii

tr A A

11 12

12T

A AA A A

0Tx Ax ( 0)Tx Ax

Eigenvalues and Eigenvectors of a matrix For a matrix A, the vector x is an eigenvector of A

with a corresponding eigenvalue λ if they satisfy the equation

The eigenvalues of a diagonal matrix are its diagonal elements

The inverse of A exists if and only if (iff) none of the eigenvalues are zero

Positive definite A has all eigenvalues greater than zero7

LINEAR ALGEBRA

Ax x

Differentiation of linear matrix equation

Differentiation of a quadratic matrix equation

8

LINEAR ALGEBRA

T T

d Ax Adxd x A Adx

T T T Td x Ax x A x Adx

Least Squares Solution If A is a skinny matrix (n>m), and we wish to find x

for which

Since A is skinny, the problem is over-constrained No solution exists

Instead, minimize the square of the error between Axand b

9

LINEAR ALGEBRA

Ax b

22min || ||

min

min 2

xT

xT T T T

x

Ax b

Ax b Ax b

x A Ax b Ax b b

Setting the derivative to zero

Known as the pseudo-inverse

This methodology is used over and over in the course Quadratic cost minimized to find closed form solution

10

LINEAR ALGEBRA

1

†

2 2 0T T T

T T

T T

x A A b AA Ax A b

x A A A b

x A b

Least Squares example Data fitting with polynomials

Given a function of interest

And a set of measurements b(tm)of that function at points tm

Find the best polynomial fit for polynomial fP(t) of order P

11

LINEAR ALGEBRA

2

1( )1 25

g tt

( ), { 1, 0.99,...,1}m mb t t

1 2 1( ) PP Pf t x x t x t

Least Squares example Can formulate this as a least squares problem where

we want to minimize the mean square error between polynomial prediction and measurement at each tm:

The polynomial can be written as

Use least squares solution method to find coefficients of f.12

LINEAR ALGEBRA

22min || ( ) ( ) ||P m mx

f t b t

21 1 1

22 2 2

2

11

( )

1

P

P

m

Pm m m

t t tt t t

A t

t t t

1 2 1( ) ( ) PP m m m P mf t A t x x x t x t

1

2

P

xx

x

x

13

5th order10 Points

15th order10 Points

5th order100 Points

15th order100 Points

SISO CONTROL

One input, one output, one transfer function between the two

Model requires transfer function from single input to single output initial conditions to start from (usually assumed 0)

Model hides inner workings of plant

( ) ( )( )

Y s T sR s

( )cKG s ( )pG s

( )R s

( )E s ( )Y s( )U sController Plant

Closed loop transfer function

14

STATE SPACE: A NEW SYSTEM MODEL

Multi-Input-Multi-Output (MIMO) model, maintains complete plant picture Matrix and vector notation, use power of linear algebra for

many key results

Definition: The state of a system is a vector of system variables that entirely defines the system at a specific instance in time. Example: at t=0, initial conditions define a state vector. Entire history of state variables can be discarded, only

need current state and system dynamics to continue forward in time. 15

STANDARD FORM DYNAMICS

Linear first order time-invariant dynamics in continuous time Update equation

Measurement equation

A, B matrices: state derivatives can depend on any state or input variable

C, D matrices: output can depend on any state or input variable

( ) ( ) ( )x t Ax t Bu t

( ) ( ) ( )y t Cx t Du t

16

STANDARD FORM DYNAMICS

Linear first order time-invariant dynamics in discrete time Update equation, timesteps indexed by t

Measurement equation

A, B matrices: state update can depend on any state or input variable

C, D matrices: measurements can depend on any state or input variable

1t t tx Ax Bu

t t ty Cx Du

17

Equation of Motion

Transfer function

Four variables in ODE: one input, one variable defined by ODE, two states remain.

EXAMPLE – SPRING MASS DAMPERz(t)

f (t)m

k

b( ) ( ) ( ) ( )mz t bz t kz t f t

2( ) 1( )

Z sF s ms bs k

1

2

( ) ( )( )

( ) ( )x t z t

x tx t z t

18

Motion Model

Measurement model: position and velocity sensors

EXAMPLE CONT’Dz(t)

f (t)m

k

b

0 1 0( ) ( )( )1( ) ( )

( ) ( ) ( )

z t z tf tk bz t z t

m m mx t Ax t Bu t

1

2

( ) 1 0 ( ) 0( )

( ) 0 1 ( ) 0( ) ( )

y t z tf t

y t z ty Cx t Du t

19

IN TRANSFER FUNCTION FORM

Take Laplace transform of update equation

Solve for X(s), with x(0)=0

Combine with measurement model

( ) ( ) ( )dx t Ax t Bu tdt

( ) (0) ( ) ( )sX s x AX s BU s

1

( ) ( ) ( )( ) ( ) ( )

sI A X s BU sX s sI A BU s

1

( ) ( ) ( )( )( )

Y s CX s DU sY s C sI A B DU s

20

BLOCK DIAGRAM

Continuous LTI State space model as a block diagram (Laplace Domain)

( )X s ( )Y s1s

( )U s

Plant

A

B C

D

( )sX s

21

EXAMPLE: FIND TFSz(t)

f (t)m

k

b

1

1

1

( ) ( )( )

0 1 01 0 0( )10 1 0( )

1 01 0( )10 1( )

Y s C sI A BU s

sY sk bsU s

m m m

sY s

k bU s sm m m

0 1 0 1 0 0, , ,

1 0 1 0A B C Dk b

m m

22

EXAMPLE: FIND TFSz(t)

f (t)m

k

b

2

2

011 0( ) 110 1( ) det( )

1

1det( )( )

( )det( )

bsY s m

kU s sI A s mm

mb ks ssI AY s m m

s sU smsI Ab ks sm m

So roots of det(sI-A) determine poles of open loop system Well known equation in linear algebra: eigenvalues/eigenvectors Open loop poles are eigenvalues of A Holds for all sizes of A, not just 2X2

23

STATE FEEDBACK CONTROL

If C = I and D = 0, full state feedback More than one signal, in fact everything we could possibly

need

Assume R(s) = 0 (regulator)

( ) ( )u t Kx t

K

( )X s( )R s

( )E s ( )Y s1s

( )U s

Controller

Plant

A

B C

D

( )sX s

24

STATE FEEDBACK

Closed loop transfer function

Now, eigenvalues of A-BK are poles of closed loop system

In fact, since there is one K for every eigenvalue, we can place the closed loop poles anywhere we’d like.

1( ) ( )( )

X s sI A BKR s

25

EXAMPLE: POLE PLACEMENT

Lets place closed loop poles at Note:

Match coefficients of polynomials Desired = actual

5 5s j

1 2 1 2

0 0 01BK K K K Km m m

21 2

2 2 1

110 50 det

ss s k K b Ks

m m m mK b K ks sm m m m

z(t)

f (t)M

K

B

26

EXAMPLE: CONTROLLER

What is full state feedback?

PD control To add integral control, add an

integrator state

1 2( ) ( ) ( )u t K z t K z t z(t)

f (t)M

K

B

( )

zdt

x t zz

0 1 0 00 0 1 , 0

10

A Bk bm m m

27

CONTROLLABILITY

Can I get there from here?

A system is controllable if for any set of initial and final states, x(0) and x(T), there exists a control input sequence, u(0) to u(T), to get from x(0) to x(T).

Can be checked easily: The following matrix must be full rank

2 1rank nB AB A B A B n

28

OBSERVABILITY

Can I see there from here? Given any sequence of states x(0) to x(T), inputs u(0)

to u(T) and outputs y(0) to y(T), a system is observable if the state can be uniquely determined from the outputs alone.

Again, an easy check on the observability matrix determines if a system is observable

2

1

rank

n

CCACA n

CA

29

OPTIMAL CONTROL

Since we can place poles anywhere, can change objective of control design

Minimize quadratic errors in states and quadratic use of inputs

Penalize big deviations more heavily that small ones Quadratic cost and linear dynamics result in a time

invariant control law, another way to set the gains for state feedback control

0,min ( ) ( ) ( ) ( )

t T T

x ux Qx u Ru d

30

OPTIMAL ESTIMATION

The second half of the model describes the relationship between the state and the measured outputs. Any sensor dynamics must be included in the state

In reality, both disturbances and noise will exist

Assume w, v are Gaussian white noise with covariance Q, R

Assume u(t) is known exactly Formulate minimum mean squared error estimation

problem, results in Kalman filter

( ) ( ) ( ) ( )x t Ax t Bu t w t

( ) ( ) ( ) ( )y t Cx t Du t v t

31

STATE SPACE MODEL

Estimator

K( )X s

1s

( )U s

Controller

Plant

A

B

C

( )Y s

( )eG sˆ ( )X s

( )V s( )W s

32

( )R s

D