MIT OpenCourseWare http://ocw.mit.edu

16.323 Principles of Optimal Control Spring 2008

For information about citing these materials or our Terms of Use, visit: http://ocw.mit.edu/terms.

16.323 Lecture 11

Estimators/Observers

• Bryson Chapter 12• Gelb – Optimal Estimation

Spr 2008 16.323 11–1 Estimators/Observers

Problem: So far we have assumed that we have full access to the • state x(t) when we designed our controllers. – Most often all of this information is not available.

– And certainly there is usually error in our knowledge of x.

• Usually can only feedback information that is developed from the sen­

sors measurements. – Could try “output feedback” u = Kx u = Ky⇒

– But this is type of controller is hard to design.

• Alternative approach: Develop a replica of the dynamic system that provides an “estimate” of the system states based on the measured output of the system.

• New plan: called a “separation principle” 1. Develop estimate of x(t), called x(t).

2. Then switch from u = −Kx(t) to u = −Kx(t).

• Two key questions: – How do we find x(t)?

– Will this new plan work? (yes, and very well)

June 18, 2008

�

Spr 2008 Estimation Schemes 16.323 11–2

• Assume that the system model is of the form:

x = Ax + Bu , x(0) unknown

y = Cyx

where – A, B, and Cy are known – possibly time-varying, but that is sup­

pressed here. – u(t) is known – Measurable outputs are y(t) from Cy = I

• Goal: Develop a dynamic system whose state

x(t) = x(t) ∀t ≥ 0

Two primary approaches: – Open-loop. – Closed-loop.

June 18, 2008

Spr 2008 Open-loop Estimator 16.323 11–3

• Given that we know the plant matrices and the inputs, we can just perform a simulation that runs in parallel with the system

x(t) = Ax + Bu(t)

– Then x(t) ≡ x(t) ∀ t provided that x(0) = x(0)

System A,B,Cy

⇒ x(t)

y(t) ��

u(t)

��

��Observer A,B,Cy

⇒ x(t)

y(t) ��

• To analyze this case, start with:

x(t) = Ax(t) + Bu(t)

x(t) = Ax(t) + Bu(t)

• Define the estimation error: x(t) = x(t) − x(t). – Now want x(t) = 0 ∀ t, but is this realistic?

• Major Problem: We do not know x(0)

June 18, 2008

�

Spr 2008 16.323 11–4

• Subtract to get:

d dt

(x − x) = A(x − x) ⇒ x(t) = Ax

which has the solution

x(t) = eAt x(0)

– Gives the estimation error in terms of the initial error.

• Does this guarantee that x = 0 ∀ t? Or even that x 0 as t →∞? (which is a more realistic goal). →

– Response is fine if x(0) = 0. But what if x(0) = 0?

• If A stable, then x → 0 as t →∞, but the dynamics of the estimation error are completely determined by the open-loop dynamics of the system (eigenvalues of A). – Could be very slow.

– No obvious way to modify the estimation error dynamics.

• Open-loop estimation is not a very good idea.

June 18, 2008

Spr 2008 Closed-loop Estimator 16.323 11–5

Obvious fix to problem: use the additional information available:• – How well does the estimated output match the measured output?

Compare: y = Cyx with y = Cyx

– Then form y = y − y ≡ Cy x

System A,B,Cy

→ x(t)

y(t) ��

+����

��

u(t)

��

��

L

Observer A,B,Cy

→ x(t)

y(t) ��

−

• Approach: Feedback y to improve our estimate of the state. Basic form of the estimator is:

x(t) = Ax(t) + Bu(t) + Ly(t)

y(t) = Cyx(t)

where L is a user selectable gain matrix.

Analysis:•

x = x− x = [Ax + Bu] − [Ax + Bu + L(y − y)]

= A(x − x) − L(Cx − Cyx)

= Ax− LCyx = (A − LCy)x

June 18, 2008

Spr 2008 16.323 11–6

• So the closed-loop estimation error dynamics are now

x = (A − LCy)x with solution x(t) = e(A−LCy)t x(0)

• Bottom line: Can select the gain L to attempt to improve the convergence of the estimation error (and/or speed it up). – But now must worry about observability of the system [A,Cy].

• Note the similarity: – Regulator Problem: pick K for A − BK �Choose K ∈ R1×n (SISO) such that the closed-loop poles

det(sI − A + BK) = Φc(s)

are in the desired locations. – Estimator Problem: pick L for A − LCy

�Choose L ∈ Rn×1 (SISO) such that the closed-loop poles

det(sI − A + LCy) = Φo(s)

are in the desired locations.

• These problems are obviously very similar – in fact they are called dual problems

– Note: poles of (A − LCy) and (A − LCy)T are identical.

– Also have that (A − LCy)T = AT − CTLT y

– So designing LT for this transposed system looks like a standard regulator problem (A − BK) where

A AT ⇒ B CT ⇒ y

K LT ⇒

June 18, 2008

� �

� � � �

• � �

Spr 2008 16.323 11–7 Estimator Example 10–1

• Simple system (see page 11-23) � � � � � � −1 1.5 1 −0.5 A = , B = , x(0) =

1 −2 0 −1

Cy = 1 0 , D = 0

– Assume that the initial conditions are not well known.

– System stable, but λmax(A) = −0.18

– Test observability:

Cy 1 0 rank = rank

CyA −1 1.5

Use open and closed-loop estimators. Since the initial conditions are 0

not well known, use x(0) = 0

• Open-loop estimator:

x = Ax + Bu

y = Cyx

• Closed-loop estimator:

x = Ax + Bu + Ly = Ax + Bu + L(y − y)

= (A − LCy)x + Bu + Ly

y = Cyx

– Dynamic system with poles λi(A − LCy) that takes the measured plant outputs as an input and generates an estimate of x.

– Use place command to set closed-loop pole locations

June 18, 2008

Spr 2008 16.323 11–8

• Typically simulate both systems together for simplicity

• Open-loop case:

x = Ax + Bu

y = Cyx

x = Ax + Bu

y = Cyx ⎡ ⎤ � � � � � � � � � � ⎢ −0.5 ⎥ x

= A 0 x

+ B

u , x(0)

= ⎢ −1 ⎥ ⇒x 0 A x B x(0)

⎢⎣ 0 ⎥⎦

0 � � � � � � y Cy 0 x

= y 0 Cy x

• Closed-loop case:

x = Ax + Bu

x = (A − LCy)x + Bu + LCyx � � � � � � � � x A 0 x B ⇒ x

= LCy A − LCy x

+ B

u

• Example uses a strong u(t) to shake things up

June 18, 2008

Spr 2008 16.323 11–9

0 0.5 1 1.5 2 2.5 3 3.5 4−1

−0.5

0

0.5

1Open loop estimator

stat

es

time

x1 x

2

0 0.5 1 1.5 2 2.5 3 3.5 4−1

−0.5

0

0.5

1

estim

atio

n er

ror

time

Figure 11.1: Open-loop estimator. Estimation error converges to zero, but veryslowly.

0 0.5 1 1.5 2 2.5 3 3.5 4−1

−0.5

0

0.5

1Closed−loop estimator

stat

es

time

x1 x

2

0 0.5 1 1.5 2 2.5 3 3.5 4−1

−0.5

0

0.5

1

estim

atio

n er

ror

time

Figure 11.2: Closed-loop estimator. Convergence looks much better.

June 18, 2008

Spr 2008 16.323 11–10 Estimator Poles? • Location heuristics for poles still apply – use Bessel, ITAE, . . .

– Main difference: probably want to make the estimator faster than you intend to make the regulator – should enhance the control, which is based on x(t).

– ROT: Factor of 2–3 in the time constant ζωn associated with the regulator poles.

• Note: When designing a regulator, were concerned with “bandwidth” of the control getting too high often results in control commands ⇒ that saturate the actuators and/or change rapidly.

Different concerns for the estimator: • – Loop closed inside computer, so saturation not a problem.

– However, the measurements y are often “noisy”, and we need to be careful how we use them to develop our state estimates.

⇒ High bandwidth estimators tend to accentuate the effect of sens­

ing noise in the estimate. – State estimates tend to “track” the measurements, which are fluc­

tuating randomly due to the noise.

⇒ Low bandwidth estimators have lower gains and tend to rely more heavily on the plant model – Essentially an open-loop estimator – tends to ignore the measure­

ments and just uses the plant model.

June 18, 2008

Spr 2008 Optimal Estimator 16.323 11–11

• Can also develop an optimal estimator for this type of system.

– Given duality of regulator and estimator, would expect to see close connection between optimal estimator and regulator (LQR)

• Key step is to balance the effect of the various types of random noise in the system on the estimator:

x = Ax + Bu + Bww

y = Cyx + v

– w: “process noise” – models uncertainty in the system model. – v: “sensor noise” – models uncertainty in the measurements.

• Typically assume that w(t) and v(t) are zero mean E[w(t)] = 0 and – Uncorrelated Gaussian white random noises: no correlation between

the noise at one time instant and another

E[w(t1)w(t2)T ] =Rww(t1)δ(t1 − t2) ⇒ w(t) ∼ N (0, Rww)

E[v(t1)v(t2)T ] =Rvv(t1)δ(t1 − t2) ⇒ v(t) ∼ N (0, Rvv)

E[w(t1)v(t2)T ] =0

−20 −15 −10 −5 0 5 10 15 20

10−2

10−1

100

X

P(X

)

Gaussian for various Sigma

0.25

0.5

1

5

10

25

Figure 11.3: Example of impact of covariance = σ2 on the distribution of the PDF – wide distribution corresponds to large uncertainty in the variable

June 18, 2008

Spr 2008 Analysis 16.323 11–12

• With noise in the system, the model is of the form:

x = Ax + Bu + Bww , y = Cyx + v

– And the estimator is of the form:

x = Ax + Bu + L(y − y) , y = Cyx

• Analysis: in this case:

x = x− x = [Ax + Bu + Bww] − [Ax + Bu + L(y − y)]

= A(x − x) − L(Cyx − Cyx) + Bww − Lv

= Ax− LCyx + Bww − Lv

= (A − LCy)x + Bww − Lv (11.18)

• This equation of the estimation error explicitly shows the conflict in the estimator design process. Must balance between: – Speed of the estimator decay rate, which is governed by

Re[λi(A − LCy)]

– Impact of the sensing noise v through the gain L

• Fast state reconstruction requires rapid decay rate – typically requires a large L, but that tends to magnify the effect of v on the estimation process. – The effect of the process noise is always there, but the choice of L

will tend to mitigate/accentuate the effect of v on x(t).

• Kalman Filter needs to provide an optimal balance between the two conflicting problems for a given “size” of the process and sensing noises.

June 18, 2008

Spr 2008 Optimization 16.323 11–13

• Note that Eq. 11.18 is of the form of a linear time-varying system driven by white Gaussian noise – Can predict the mean square value of the state (estimation error

in this case) Q(t) = E[x(t)x(t)T ] over time using Q(0) = Q0 and

Q(t) = [A − LCy] Q(t) + Q(t) [�A − LCy]T � � � � � Rww 0 BT

+ wBw −L 0 Rvv −LT

= [A − LCy] Q(t) + Q(t) [A − LCy]T + BwRwwBw

T + LRvvLT

– Called a matrix differential Lyapunov Equation16

• Note that ideally would like to minimize Q(t) or trace Q(t), but that is difficult to do & describe easily17 .

Instead, consider option of trying to minimize trace Q(t), the argu­•ment being that then

� 0 t trace Q(τ )dτ is small.

– Not quite right, but good enough to develop some insights

• To proceed note that

∂ ∂ trace[AXB] = trace[BTXTAT ] = ATBT

∂X ∂X and

∂ trace[AXBXTC] = ATCTXBT + CAXB

∂X

• So for minimum we require that

∂ trace Q = −2QTCT + 2LRvv = 0

∂L y

which implies that L = Q(t)CTR−1

y vv

16See K+S, chapter 1.11 for details.

17My 16.324 discuss how to pose the problem in discrete time and then let Δt 0 to recover the continuous time results. →

June 18, 2008

Spr 2008 16.323 11–14

• Note that if we use this expression for L in the original differential Lyapunov Equation, we obtain:

Q(t) = [A − LCy] Q(t) + Q(t) [A − LCy]T + BwRwwB

T + LRvvLT

w

= � A − Q(t)CTR−1Cy

� Q(t) + Q(t)

� A − Q(t)CTR−1Cy

�T y vv y vv

+BwRwwBT + Q(t)CTR−1Rvv(Q(t)CTR−1)T w y vv y vv

= AQ(t) + Q(t)AT − 2Q(t)CTR−1CyQ(t) + BwRwwBT

y vv w

+Q(t)CyT

vv CyQ(t)R−1

Q(t) = AQ(t) + Q(t)AT BT R−1 Q(t)+ BwRww w − Q(t)CyT

vv Cy

which is obviously a matrix differential Riccati equation.

June 18, 2008

� �

Spr 2008 Optimal Kalman Filter 16.323 11–15

• Goal: develop an estimator x(t) which is a linear function of the measurements y(τ ) (0 ≤ τ ≤ t) and minimizes the function

J = trace(Q(t))

Q(t) = E {x(t) − x(t)}{x(t) − x(t)}T

which is the covariance for the estimation error.

• Solution: is a closed-loop estimator 18

x(t) = Ax + L(t)(y(t) − Cyx(t))

where L(t) = Q(t)CTR−1 and Q(t) ≥ 0 solves y vv

Q(t) = AQ(t) + Q(t)AT + Bw BT − Q(t)CTR−1 Q(t)Rww w y vv Cy

– Note that x(0) and Q(0) are known

– Differential equation for Q(t) solved forward in time.

– Filter form of the differential matrix Riccati equation for the error covariance.

– Note that the AQ(t) + Q(t)AT . . . is different than with the regu­

lator which had P (t)A + ATP (t) . . .

• Called Kalman-Bucy Filter – linear quadratic estimator (LQE)

18See OCW notes for 16.322 “Stochastic Estimation and Control” for the details of this derivation.

June 18, 2008

Spr 2008 16.323 11–16

• Note that an increase in Q(t) corresponds to increased uncertainty in the state estimate. Q(t) has several contributions: – AQ(t) + Q(t)AT is the homogeneous part

– BwRwwBwT increase due to the process measurements

– Q(t)CTR−1CyQ(t) decrease due to measurements y vv

• The estimator gain is L(t) = Q(t)CTR−1 y vv

– Feedback on the innovation, y − y

– If the uncertainty about the state is high, then Q(t) is large, and so the innovation y − Cyx is weighted heavily (L )↑

– If the measurements are very accurate Rvv , then the measure­↓ments are heavily weighted

June 18, 2008

Spr 2008 Steady State 16.323 11–17

Assume that 19 • 1. Rvv > 0, Rww > 0

2. All plant dynamics are constant in time

3. [A,Cy] detectable

4. [A,Bw] stabilizable

• Then, as with the LQR problem, the covariance of the LQE quickly settles down to a constant Qss independent of Q(0), as t →∞ where

AT BT CTR−1AQss + Qss + BwRww w − Qss y vv CyQss = 0

– Stabilizable/detectable gives a unique Qss ≥ 0

– Qss > 0 iff [A,Bw] controllable

– Lss = QssCTR−1 y vv

• If Qss exists, the steady state filter

x(t) = Ax + Lss(y(t) − Cyx(t))

= (A − LssCy)x(t) + Lssy(t)

is asymptotically stable iff (1)–(4) above hold.

19Compare this with 4–10

June 18, 2008

Spr 2008 Filter Interpretation 16.323 11–18

Given that x = (A − LCy)x + Ly•

• Consider a scalar system, and take the Laplace transform of both sides to get:

X(s) L =

Y (s) sI − (A − LCy)

This is the transfer function from the “measurement” to the “esti­• mated state” – It looks like a low-pass filter.

• Clearly, by lowering Rvv, and thus increasing L, we are pushing out the pole. – DC gain asymptotes to 1/Cy as L →∞

10−1

100

101

102

103

104

105

106

10−2

10−1

100

Scalar TF from Y to \hat X for larger L

Freq (rad/sec)

|\hat

X /

Y|

Increasing L

June 18, 2008

Spr 2008 Example 10–2 16.323 11–19

• Lightly Damped Harmonic Oscillator � � � � � � � � x1 0 1 x1 0

= + w x2 −ω2 0 x2 1

0

and y = x1 + v, where Rww = 1 and Rvv = r.

– Can sense the position state of the oscillator, but want to develop an estimator to reconstruct the velocity state.

• Symmetric root locus exists for the optimal estimator. Can find location of the optimal poles using a SRL based on the TF

� � � s −1

�−1 � 0 �

1 N(s)Gyw(s) = 1 0

ω02 s 1

= s2 + ω0

2 = D(s)

– SRL for the closed-loop poles λi(A − LC) of the estimator which are the LHP roots of:

RwwD(s)D(−s) ± N(s)N(−s) = 0

Rvv

– Pick sign to ensure that there are no poles on the jω-axis (other than for a gain of zero)

– So we must find the LHP roots of � � � � 1 1 s 2 + ω2 (−s)2 + ω2 + = (s 2 + ω0

2)2 + = 0 0 0 r r

−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1−1.5

−1

−0.5

0

0.5

1

1.5

Real Axis

Imag

Axi

s

Symmetric root locus

June 18, 2008

� � � �� � � �

� � �

�

Spr 2008 16.323 11–20

• Note that as r → 0 (clean sensor), the estimator poles tend to ∞ along the ±45 deg asymptotes, so the poles are approximately

s ≈ −1 √± r

j ⇒ Φe(s) = s2 2 2+ s + = 0√

r r

• Can use these estimate pole locations in acker, to get that ⎛ ⎞� �2 � � � �−1 � �0 1 2 0 1 2 C 0

L = ⎝ + + I⎠√r r CA 12

0 −ω20−ω 0 0

2√r

� �−1 � � 0

2r

− ω

2√r

20

20− ω

2√r

1 0= = 2− ω20

2r

20

0 1 1ω− r

22

• Given L, A, and C, we can develop the estimator transfer function from the measurement y to the x2 � � � � � �

√r

√r

−1 �� � 0 1x20 1 sI − 1 0+= 2 22

0−ω20−� ω − ωy 0

2

r

√r

r�−1 �

rr� �� 2

2√r

s

√r

−1� � s += 0 1 2 2

02 − ω�� � s 1

2

1 √r

2

= 0 1 −2 r

√r

2r− ω 22

02√r

− ω

s2 + s +2

s +

√r

r

22

s −√rω

√r r

√r r

• Filter zero asymptotes to s = 0 as r → 0 and the two poles →∞

• Resulting estimator looks like a “band-limited” differentiator. – Expected because we measure position and want to estimate veloc­

ity.

– Frequency band over which filter performs differentiation deter­

mined by the “relative cleanliness” of the measurements.

June 18, 2008

−2 r )(2

r20+ (s + ) 2

0≈= 2 2s2 + s2 +s + s +

20

Spr 2008 16.323 11–21

10−4

10−3

10−2

10−1

100

101

102

103

104

10−4

10−3

10−2

10−1

100

101

102

103

104

Freq (rad/sec)

Mag

Vel sens to Pos state, sen noise r=1e−008

r=10−2

r=10−4

r=10−6

r=10−8

10−4

10−3

10−2

10−1

100

101

102

103

104

0

20

40

60

80

100

120

140

160

180

200

Freq (rad/sec)

Pha

se (

deg)

r=10−2

r=10−4

r=10−6

r=10−8

Figure 11.4: Bandlimited differentiation of the position measurement from LQE: r = 10−2 , r = 10−4 , r = 10−6, and r = 10−8

June 18, 2008

Spr 2008 16.323 11–22 Final Thoughts

• Note that the feedback gain L in the estimator only stabilizes the estimation error. – If the system is unstable, then the state estimates will also go to ∞, with zero error from the actual states.

• Estimation is an important concept of its own. – Not always just “part of the control system”

– Critical issue for guidance and navigation system

• More complete discussion requires that we study stochastic processes and optimization theory.

• Estimation is all about which do you trust more: your measurements or your model.

• Strong duality between LQR and LQE problems

A AT ↔ B CT ↔ y

Cz ↔ BwT

Rzz Rww↔ Ruu Rvv↔ K(t) ↔ LT (tf − t)P (t) ↔ Q(tf − t)

June 18, 2008

Spr 2008 16.323 11–23

Basic Estimator (examp1.m) (See page 11-7)

1 % Examples of estimator performance 2 % Jonathan How, MIT 3 % 16.333 Fall 2005 4 % 5 % plant dynamics 6 % 7 a=[-1 1.5;1 -2];b=[1 0]’;c=[1 0];d=0; 8 % 9 % estimator gain calc

10 % 11 l=place(a’,c’,[-3 -4]);l=l’ 12 % 13 % plant initial cond 14 xo=[-.5;-1]; 15 % extimator initial cond 16 xe=[0 0]’; 17 t=[0:.1:10]; 18 % 19 % inputs 20 % 21 u=0;u=[ones(15,1);-ones(15,1);ones(15,1)/2;-ones(15,1)/2;zeros(41,1)]; 22 % 23 % open-loop extimator 24 % 25 A_ol=[a zeros(size(a));zeros(size(a)) a]; 26 B_ol=[b;b]; 27 C_ol=[c zeros(size(c));zeros(size(c)) c]; 28 D_ol=zeros(2,1); 29 % 30 % closed-loop extimator 31 % 32 A_cl=[a zeros(size(a));l*c a-l*c];B_cl=[b;b]; 33 C_cl=[c zeros(size(c));zeros(size(c)) c];D_cl=zeros(2,1); 34

35 [y_cl,x_cl]=lsim(A_cl,B_cl,C_cl,D_cl,u,t,[xo;xe]); 36 [y_ol,x_ol]=lsim(A_ol,B_ol,C_ol,D_ol,u,t,[xo;xe]); 37

38 figure(1);clf;subplot(211) 39 plot(t,x_cl(:,[1 2]),t,x_cl(:,[3 4]),’--’,’LineWidth’,2);axis([0 4 -1 1]); 40 title(’Closed-loop estimator’);ylabel(’states’);xlabel(’time’) 41 text(.25,-.4,’x_1’);text(.5,-.55,’x_2’);subplot(212) 42 plot(t,x_cl(:,[1 2])-x_cl(:,[3 4]),’LineWidth’,2) 43 %setlines; 44 axis([0 4 -1 1]);grid on 45 ylabel(’estimation error’);xlabel(’time’) 46

47 figure(2);clf;subplot(211) 48 plot(t,x_ol(:,[1 2]),t,x_ol(:,[3 4]),’--’,’LineWidth’,2);axis([0 4 -1 1]) 49 title(’Open loop estimator’);ylabel(’states’);xlabel(’time’) 50 text(.25,-.4,’x_1’);text(.5,-.55,’x_2’);subplot(212) 51 plot(t,x_ol(:,[1 2])-x_ol(:,[3 4]),’LineWidth’,2) 52 %setlines; 53 axis([0 4 -1 1]);grid on 54 ylabel(’estimation error’);xlabel(’time’) 55

56 print -depsc -f1 est11.eps; jpdf(’est11’) 57 print -depsc -f2 est12.eps; jpdf(’est12’)

June 18, 2008

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

Spr 2008 16.323 11–24

Filter Interpretation

% Simple LQE example showing SRL% 16.323 Spring 2007% Jonathan How%a=[0 1;-4 0];c=[1 0]; % pos sensorc2=[0 1]; % vel state outf=logspace(-4,4,800);

r=1e-2;l=polyvalm([1 2/sqrt(r) 2/r],a)*inv([c;c*a])*[0 1]’[nn,dd]=ss2tf(a-l*c,l,c2,0); % to the vel estimateg=freqresp(nn,dd,f*j);[r roots(nn)]figure(1)subplot(211)f1=f;g1=g;loglog(f,abs(g))%hold on;fill([5e2 5e2 1e3 1e3 5e2]’,[1e4 1e-4 1e-4 1e4 1e4]’,’c’);hold offxlabel(’Freq (rad/sec)’)ylabel(’Mag’)title([’Vel sens to Pos state, sen noise r=’,num2str(r)])axis([1e-3 1e3 1e-4 1e4])subplot(212)semilogx(f,unwrap(angle(g))*180/pi)xlabel(’Freq (rad/sec)’)ylabel(’Phase (deg)’)axis([1e-3 1e3 0 200])

figure(2)r=1e-4;l=polyvalm([1 2/sqrt(r) 2/r],a)*inv([c;c*a])*[0 1]’[nn,dd]=ss2tf(a-l*c,l,c2,0); % to the vel estimateg=freqresp(nn,dd,f*j);[r roots(nn)]subplot(211)f2=f;g2=g;loglog(f,abs(g))%hold on;fill([5e2 5e2 1e3 1e3 5e2]’,[1e4 1e-4 1e-4 1e4 1e4]’,’c’);hold offxlabel(’Freq (rad/sec)’)ylabel(’Mag’)title([’Vel sens to Pos state, sen noise r=’,num2str(r)])axis([1e-3 1e3 1e-4 1e4])subplot(212)semilogx(f,unwrap(angle(g))*180/pi)xlabel(’Freq (rad/sec)’)ylabel(’Phase (deg)’)%bode(nn,dd);axis([1e-3 1e3 0 200])

figure(3)r=1e-6;l=polyvalm([1 2/sqrt(r) 2/r],a)*inv([c;c*a])*[0 1]’[nn,dd]=ss2tf(a-l*c,l,c2,0); % to the vel estimateg=freqresp(nn,dd,f*j);[r roots(nn)]subplot(211)f3=f;g3=g;loglog(f,abs(g))%hold on;fill([5e2 5e2 1e3 1e3 5e2]’,[1e4 1e-4 1e-4 1e4 1e4]’,’c’);hold offxlabel(’Freq (rad/sec)’)ylabel(’Mag’)title([’Vel sens to Pos state, sen noise r=’,num2str(r)])axis([1e-3 1e3 1e-4 1e4])subplot(212)semilogx(f,unwrap(angle(g))*180/pi)xlabel(’Freq (rad/sec)’)

June 18, 2008

Spr 2008 16.323 11–25

68 ylabel(’Phase (deg)’)69 %bode(nn,dd);70 title([’Vel sens to Pos state, sen noise r=’,num2str(r)])71 axis([1e-3 1e3 0 200])72

73 figure(4)74 r=1e-8;75 l=polyvalm([1 2/sqrt(r) 2/r],a)*inv([c;c*a])*[0 1]’76 [nn,dd]=ss2tf(a-l*c,l,c2,0); % to the vel estimate77 g=freqresp(nn,dd,f*j);78 [r roots(nn)]79 f4=f;g4=g;80 subplot(211)81 loglog(f,abs(g))82 %hold on;fill([5e2 5e2 1e3 1e3 5e2]’,[1e4 1e-4 1e-4 1e4 1e4]’,’c’);hold off83 xlabel(’Freq (rad/sec)’)84 ylabel(’Mag’)85 title([’Vel sens to Pos state, sen noise r=’,num2str(r)])86 axis([1e-3 1e3 1e-4 1e4])87 title([’Vel sens to Pos state, sen noise r=’,num2str(r)])88 subplot(212)89 semilogx(f,unwrap(angle(g))*180/pi)90 xlabel(’Freq (rad/sec)’)91 ylabel(’Phase (deg)’)92 %bode(nn,dd);93 axis([1e-3 1e3 0 200])94

95 print -depsc -f1 filt1.eps; jpdf(’filt1’)96 print -depsc -f2 filt2.eps;jpdf(’filt2’)97 print -depsc -f3 filt3.eps;jpdf(’filt3’)98 print -depsc -f4 filt4.eps;jpdf(’filt4’)99

100 figure(5);clf 101 %subplot(211) 102 loglog(f1,abs(g1),f2,abs(g2),f3,abs(g3),f4,abs(g4),’Linewidth’,2) 103 %hold on;fill([5e2 5e2 1e3 1e3 5e2]’,[1e4 1e-4 1e-4 1e4 1e4]’,’c’);hold off 104 xlabel(’Freq (rad/sec)’) 105 ylabel(’Mag’) 106 title([’Vel sens to Pos state, sen noise r=’,num2str(r)]) 107 axis([1e-4 1e4 1e-4 1e4]) 108 title([’Vel sens to Pos state, sen noise r=’,num2str(r)]) 109 legend(’r=10^{-2}’,’r=10^{-4}’,’r=10^{-6}’,’r=10^{-8}’,’Location’,’NorthWest’) 110 %subplot(212) 111 figure(6);clf 112 semilogx(f1,unwrap(angle(g1))*180/pi,f2,unwrap(angle(g2))*180/pi,... 113 f3,unwrap(angle(g3))*180/pi,f4,unwrap(angle(g4))*180/pi,’Linewidth’,2);hold off 114 xlabel(’Freq (rad/sec)’) 115 ylabel(’Phase (deg)’) 116 legend(’r=10^{-2}’,’r=10^{-4}’,’r=10^{-6}’,’r=10^{-8}’) 117 %bode(nn,dd); 118 axis([1e-4 1e4 0 200]) 119 print -depsc -f5 filt5.eps;jpdf(’filt5’) 120 print -depsc -f6 filt6.eps;jpdf(’filt6’)

June 18, 2008