4 Optimal State Estimation

Size: px

Start display at page:

Download "4 Optimal State Estimation"

Stephanie Hardy
6 years ago
Views:

1 4 Optimal State Estimation Consider the LTI system ẋ(t) = Ax(t)+B w w(t), y(t) = C y x(t)+d yw w(t), z(t) = C z x(t) Problem: Compute (Â,F) such that the output of the state estimator ˆx(t) = Âˆx(t) Fy(t), ẑ(t) = C zˆx(t) stabilizes the state estimation error and minimizes the cost function J := lim t E [ (z(t) ẑ(t)) T (z(t) ẑ(t)) ]. Assumptions: a) (A,C y ) detectable; b) w(t) is a Gaussian zero mean white noise with variance W. MAE 28 B 46 Maurício de Oliveira

2 Solution: Define e(t) := x(t) ˆx(t) so that z(t) ẑ(t) = C z e(t). Now write the dynamics of the estimation error ė(t) = ẋ(t) ˆx(t), = Ax(t) Âˆx(t)+Fy(t)+B ww(t), = (A Â+FC y)x(t)+âe(t)+(b w +FD yw )w(t) First observation: if A is unstable x(t) is unbounded and so is e(t), with one exception. If and Â = A+FC y = A Â+FC y = ė(t) = (A+FC y )e(t)+(b w +FD yw )w(t). That is, the dynamics of e(t) and x(t) are decoupled! (note that z(t) ẑ(t) = C z e(t) is also decoupled from x(t)). This choice of Â produces the state observer ˆx = Aˆx+F(ŷ y), ŷ = C yˆx, ẑ = C zˆx. in which the only unknown is the observer gain F. The problem has been reduced to find the gain F. MAE 28 B 47 Maurício de Oliveira

3 Simplified problem: Compute the stabilizing state estimation gain F so as to minimize J := lim t E [ e(t) T C T z C z e(t)] where e(t) is the state of the LTI system Assumptions: a) (A,C y ) is detectable; ė(t) = (A+FC y )e(t)+(b w +FD yw )w(t). b) w(t) is a Gaussian zero mean white noise with variance W. Solution: As before where X is the Gramian J = trace [ X(B w +FD yw )W(B w +FD yw ) T] (A+FC y ) T X +X(A+FC y )+C T z C z =. However, in this form, F appears simultaneously at the cost and at the Lyapunov equation, and we can not use completion of squares to determine F. Use duality! MAE 28 B 48 Maurício de Oliveira

4 4.1 Duality Assume W. Consider the cost function J = trace ( XBWB T), where X is the solution to the Lyapunov equation A T X +XA+C T C =. An alternative (dual) expression for J can be obtained from That is J = trace ( XBWB T), ([ ) = trace e ATt C T Ce At dt ]BWB T, ( = trace Ce At BWB T e ATt C T) dt, ( [ ) = trace C e At BWB T e ATt dt ]C T. J = trace ( CYC T), where Y is the solution to the Lyapunov equation AY +YA T +BWB T =. Recall that Y is also the state covariance matrix! MAE 28 B 49 Maurício de Oliveira

5 Back to the determination of F, using the dual formulation J = trace ( ) C z YCz T where (A+FC y )Y +Y(A+FC y ) T +(B w +FD yw )W(B w +FD yw ) T =. The gain F now appears only in the Lyapunov equation! Assuming that B w WD T yw =, D yw WDT yw, (for simplicity only, without loss of generality!) the above equation becomes (A+FC y )Y +Y(A+FC y ) T +B w WB T w +FD yw WD T ywf T =. We can now complete the squares to obtain the ARE AY +YA T YC T y (D yw WD T yw) 1 C y Y +B w WB T w = and the associated optimal gain F = YC T y (D yw WD T yw) 1. MAE 28 B 5 Maurício de Oliveira

6 4.2 Summary on Estimation Problem: Given the LTI system ẋ(t) = Ax(t)+B w w(t), y(t) = C y x(t)+d yw w(t), z(t) = C z x(t) compute the estimation gain F that stabilizes the state estimation erros and that the output of the state estimator minimizes the cost function ˆx(t) = Aˆx(t)+F(ŷ(t) y(t)), ŷ(t) = C yˆx(t), ẑ(t) = C zˆx(t) J := lim t E [ (z(t) ẑ(t)) T (z(t) ẑ(t)) ]. Assumptions: a) (A,C y ) detectable; b) w(t) is a Gaussian zero mean white noise with variance W ; c) B w WDyw T =, D yw WDT yw. Solution: Find the stabilizing solution to the ARE The optimal gain is AY +YA T YC T y (D yw WDT yw ) 1 C y Y +B w WB T w =. F = YC T y (D yw WD T yw) 1. MAE 28 B 51 Maurício de Oliveira

7 Things we can infer from the form of the solution: 1) A+FC y is stable even if A is not! What happens if A is unstable and Â = A+FC y +δ? 2) The assumption D yw WDyw T is error free. reads no combination of the measurement 3) If D yw WD T yw, then part of the state could be reconstructed exactly from the measurements. While this might sounds easier, we do not know how to solve this problem :). 4) State estimation is equivalent to the state feedback problem for the LTI system minj = lim t E [ x T B w WB T w x+ut D yw WD T yw u], ẋ = A T x+c T y u+ct z w, u = F T x 5) The optimal gain F does not depend on C z! The optimal cost does! MAE 28 B 52 Maurício de Oliveira

8 4.3 Example: estimating the state of a satellite in circular orbit r u 1 u 2 m θ e Satellite of mass m with thrust in the radial direction u 1 and in the tangential direction u 2. Continuing... m( r r θ 2 ) = u 1 km r 2 +w 1, m(2ṙ θ+r θ) = u 2 +w 2, where w 1 and w 2 are independent white noise disturbances with variances δ 1 and δ 2. For the purpose of estimation we set u 1 = u 2 = for the moment. As before, putting in state space and linearizing ẋ = ω 2 2 r ω 2 ω/ r x+ 1/m 1/(m r) Consider that you have a noisy measurement of θ (x 2 ) where E{vv T } = δ 3. Problem: Given y = [ 1 ] x+v ( w1 m = 1 kg, r = R+3 km, k = GM where G N m 2 /kg 2 is the universal gravitational constant, and M kg and R km are the mass and radius of the earth. If the variances δ 1 = δ 2 = δ 3 =.1N, estimate the state of the satelite. w 2 ) MAE 28 B 53 Maurício de Oliveira

9 % MAE 28 B - Linear Control Design % Mauricio de Oliveira % % State estimation - Part I % m = 1; % 1 kg r = 3E3; % 3 km R = 6.37E6; % ˆ3 km G = 6.673E-11; % N mˆ2/kgˆ2 M = 5.98E24; % ˆ24 kg k = G * M; % gravitational force constant w = sqrt(k/((r+r)ˆ3)); % angular velocity (rad/s) v = w * (R + r); % "ground" velocity (m/s) % linearized system matrices A = [ 1 ; 1; 3*wˆ2 2*(r+R)*w; -2*w/(r+R) ]; Bu = [ ; ; 1/m ; 1/(m*(r+R))]; Bw = [ ; ; 1/m ; 1/(m*(r+R))]; % noise variances W =.1 * eye(2) W = 1.e-1 1.e-1 % any measurement that does not include x2 (theta) is not observable! % for instance Cy = [1 ; 1 ; 1]; % system sys = ss(a, Bw, Cy, ) a = x1 x2 x3 x4 x1 1 x2 1 x3 4.34e e+4 x e-1 b = u1 u2 x1 x2 x3.1 x e-9 c = x1 x2 x3 x4 y1 1 y2 1 y3 1 d = MAE 28 B 54 Maurício de Oliveira

10 u1 u2 y1 y2 y3 Continuous-time model. % take the comment out of next line to see error messages! %est = kalman(sys, W, eye(size(cy,1))); % back to the problem % measuring x2 (theta) Cy = [ r+r ]; % augment noise matrices Bwa = [Bw zeros(4,1)] Bwa = 1.e e-9 Dywa = [zeros(1,2) 1] Dywa = 1 % scale T = diag([1 r+r 1 r+r]) T = % similarity transformation At = T * A / T At = 1.e+ 1.e e e e-3 But = T * Bu But = 1.e-2 1.e-2 Bwt = T * Bwa Bwt = 1.e-2 1.e-2 Cyt = Cy / T Cyt = 1 Dywt = Dywa MAE 28 B 55 Maurício de Oliveira

11 Dywt = 1 Ww = W Ww = 1.e-1 1.e-1 Wv =.1 Wv = 1.e-1 Wt = [Ww zeros(2, 1); zeros(1, 2) Wv] Wt = 1.e-1 1.e-1 1.e-1 % compute using dual state feedback [F,X,S] = lqr(at, Cyt, Bwt * Wt * Bwt, Dywt * Wt * Dywt ); F = - F F = 8.973e e e e-2 % compute using matlab s kalman sys = ss(at, Bwt(:,1:2), Cyt, ); est = kalman(sys, Ww, Wv/rˆ2); F = -est.b F = e e+1 6.1e+3-3.4e+3 % error dynamics eig(at + F * Cyt) ans = e e e e+1i e e+1i % simulate estimator sys = ss(t * A / T, T * Bwa, Cy / T, Dywa); filt = est * sys; Tmax = 2; T = :.1 : Tmax; w = [randn(length(t),2)*sqrtm(ww) randn(length(t),1)*sqrtm(wv)]; % estimate stationary position figure(1) MAE 28 B 56 Maurício de Oliveira

12 x = [; ; ; ] x = xhat = [; ; -6; ] xhat = -6 [y,t,x] = lsim(sys, w, T, x); [yf,tf,xf] = lsim(filt, w, T, [xhat; x]); xhat = xf(:,1:4); figure(1) plot(t, x(:,[1 2]), g, t, xhat(:,[1 2]), b ), title( system and estimator trajectory (x_1 and x_2) ) xlim([, Tmax]) figure(2) plot(t, x(:,[3 4]), g, t, xhat(:,[3 4]), b ), title( system and estimator trajectory (x_3 and x_4) ) xlim([, Tmax]) figure(3) plot([ T(end)], [ ], g, t, x(:,[1 2])-xhat(:,[1 2]), b ), title( estimation error (x_1 and x_2) ) xlim([, Tmax]) figure(4) plot([ T(end)], [ ], g, t, x(:,[3 4])-xhat(:,[3 4]), b ), title( estimation error (x_3 and x_4) ) xlim([, Tmax]) diary off MAE 28 B 57 Maurício de Oliveira

13 4.4 Example: estimating position Dynamics of particle of mass m = 1 Kg in a line: mẍ = w 1, where w 1 is a zero-mean white noise disturbances with variance δ 1 = (1 N) 2. As a measurement we have a noisy measurement of position, say through GPS y 1 = x+w 2 where w 2 is a white noise disturbances with variance δ 2 = (1 m) 2. In state space [ ] [ ]( ) 1 w1 ẋ = x+, 1/m w 2 y = [ 1 ] x+ [ 1 ]( ) w 1 w 2 Problem #1: Estimate the state x. Problem #2: Can an accelerometer improve the estimate? MAE 28 B 58 Maurício de Oliveira

14 % MAE 28 B - Linear Control Design % Mauricio de Oliveira % % State estimation - Part II % m = 1; % 1 kg % linearized system matrices A = [ 1; ] A = 1 Bw = [; 1/m] Bw = 1.e-2 % position Cy = [1 ] Cy = 1 Dyw = Dyw = % noise variances Ww = 1ˆ2 Ww = 1 % augmented system Bwa = [Bw zeros(2,1)] Bwa = 1.e-2 Dywa = [zeros(1,1) 1] Dywa = 1 % measurement noise variances Wv = 1ˆ2 Wv = 1 Wa = [Ww zeros(1, 1); zeros(1, 1) Wv] Wa = 1 1 % compute using dual state feedback [F,X,S] = lqr(a, Cy, Bwa * Wa * Bwa, Dywa * Wa * Dywa ); F = - F F = MAE 28 B 59 Maurício de Oliveira

15 e-1-1.e-1 % compute using matlab s kalman sys = ss(a, Bw, Cy, Dyw); est = kalman(sys, Ww, Wv); F = -est.b F = e-1-1.e-1 % error dynamics eig(a + F * Cy) ans = e e-1i e e-1i % try to simulate estimator sys1 = ss(a, Bwa, Cy, Dywa); filt1 = est * sys1; Tmax = 24; T = :.1 : Tmax; w = [randn(length(t),1)*sqrtm(ww) randn(length(t),1)*sqrtm(wv)]; % estimate stationary position (no noise) figure(1) x = [1; ] x = 1 xhat = [; ] xhat = [y,t,x] = initial(sys1, x, T); [yf,tf,xf] = initial(filt1, [xhat; x], T); xhat = xf(:,1:2); figure(1) plot(t, x, t, xhat), title( system and estimator trajectory ) xlim([, Tmax]) figure(2) plot(t, x-xhat(:,1:2)), title( estimation error ) xlim([, Tmax]) pause % estimate stationary position (with measurement noise) wm = w; MAE 28 B 6 Maurício de Oliveira

16 wm(:,1) = ; x = [1; ] x = 1 xhat = [5; ] xhat = 5 [y,t,x] = lsim(sys1, wm, T, x); [yf,tf,xf] = lsim(filt1, wm, T, [xhat; x]); xhat = xf(:,1:2); figure(1) plot(t, x, g, t, xhat, b ), title( system and estimator trajectory ) xlim([, Tmax]) figure(2) plot([ T(end)], [ ], g, t, x-xhat(:,1:2), b ), title( estimation error ) xlim([, Tmax]) pause % estimate moving position (with measurement and process noise) x = [; ] x = xhat = zeros(2,1) xhat = [y,t,x] = lsim(sys1, w, T, x); [yf,tf,xf] = lsim(filt1, w, T, [xhat; x]); xhat = xf(:,1:2); figure(1) plot(t, x, g, t, xhat, b ), title( system and estimator trajectory ) xlim([, Tmax]) figure(2) plot([ T(end)], [ ], g, t, x-xhat(:,1:2), b ), title( estimation error ) xlim([, Tmax]) diary off MAE 28 B 61 Maurício de Oliveira

17 % MAE 28 B - Linear Control Design % Mauricio de Oliveira % % State estimation - Part III % m = 1; % 1 kg % linearized system matrices A = [ 1; ] A = 1 Bw = [; 1/m] Bw = 1.e-2 % position Cy1 = [1 ] Cy1 = 1 Dyw1 = Dyw1 = % accelerometer P = [ 1]; Cy2 = P*A Cy2 = Dyw2 = P*Bw Dyw2 = 1.e-2 Cy = [Cy1; Cy2]; Dyw = [Dyw1; Dyw2]; % noise variances Ww = 1ˆ2 Ww = 1 Wv = diag([1ˆ2 1]); % augmented system matrices Bwa = [Bw zeros(2,2)] Bwa = 1.e-2 Dywa = [zeros(2,1) eye(2)] Dywa = 1 1 Wa = [Ww zeros(1, 2); zeros(2, 1) Wv] MAE 28 B 62 Maurício de Oliveira

18 Wa = % compute using matlab s kalman sys = ss(a, Bw, Cy, Dyw); est = kalman(sys, Ww, Wv); F = -est.b F = e e e-3 % error dynamics eig(a + F * Cy) ans = e e-1i e e-1i % try to simulate estimator sys2 = ss(a, Bwa, Cy, Dywa); filt2 = est * sys2; Tmax = 24; T = :.1 : Tmax; w = [randn(length(t),1)*sqrtm(ww) randn(length(t),2)*sqrtm(wv)]; % estimate stationary position (with measurement noise) wm = w; wm(:,1) = ; x = [1; ] x = 1 xhat = [5; ] xhat = 5 [y,t,x] = lsim(sys2, wm, T, x); if exist( filt1 ) end [yf2,tf2,xf2] = lsim(filt2, wm, T, [xhat; x]); xhat2 = xf2(:,1:2); figure(1) if exist( filt1 ) else plot(t, x, g, t, xhat2, b ), end title( system and estimator trajectory ) xlim([, Tmax]) figure(2) MAE 28 B 63 Maurício de Oliveira

19 if exist( filt1 ) else plot([ T(end)], [ ], g, t, x-xhat2(:,1:2), b ), end title( estimation error ) xlim([, Tmax]) pause % estimate moving position (with measurement and process noise) x = [1; ] x = 1 xhat = [5; ] xhat = 5 [y,t,x] = lsim(sys2, w, T, x); if exist( filt1 ) end [yf2,tf2,xf2] = lsim(filt2, w, T, [xhat; x]); xhat2 = xf2(:,1:2); figure(1) if exist( filt1 ) else plot(t, x, g, t, xhat2, b ), end title( system and estimator trajectory ) xlim([, Tmax]) figure(2) if exist( filt1 ) else plot([ T(end)], [ ], g, t, x-xhat2(:,1:2), b ), end title( estimation error ) xlim([, Tmax]) diary off MAE 28 B 64 Maurício de Oliveira

9 The LQR Problem Revisited

9 The LQR Problem Revisited 9 he LQR Problem Revisited Problem: Compute a state feedback controller u(t) = Kx(t) that stabilizes the closed loop system and minimizes J := lim E [ z(t) z(t) ] t for the LI system Assumptions: a) D