ME597: AUTONOMOUS MOBILE ROBOTICS SECTION INEAR …
Transcript of ME597: AUTONOMOUS MOBILE ROBOTICS SECTION INEAR …
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