![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1320175/image.png)
LQG issue: state estimates always perfect
11 views (last 30 days)
Show older comments
I'm working through an LQG controller for a simple 2nd-order spring/mass/damper system, using an LQR controller base.
See my attached .m file.
I'm using Mathworks' LQG page here, at the section titled "Constructing the Kalman State Estimator".
Also, this link here in the section titled "Observer Design", which more clearly shows the formulation for [x;e] state vector.
For some reason, my LQG system states and state esimates track perfectly no matter what -- ie state estimates always perfectly track the state with zero error -- even if L = [0, 0]. The latter should imply no state estimate correction at all, since the actual measurements aren't taken into account to form a state estimation correction.
State esimates also perfectly track with 0 error (1e-315) on reference step changes.
What might be going on?
1) Process: Make state-space sys --> LQR design (find K) --> LQG by finding L (and using K) --> simulate
2) The state vector is [x; e], with x = [pos, vel] and e = [error_pos_hat, error_vel_hat],
where eg "error_pos_hat" is the error associated with the position state estimate.
x_dot = (A - B*K_lqr )*x + B*K_lqr*e
e_dot = 0*x + (A - L*C)*e
So:
A = [A-BK, BK; 0, A-LC]
B = [B; 0]
C = [C, 0]
D = 0
Are these state equations correct?
3) For some reason, the choice of L doesn't impact system response, even if L = [0, 0] that should imply no state estimate correction at all.
Although, the A matrix's state error eigenvalues (error_pos_hat, and error_vel_hat) do reduce as expected as L --> 0.
LQG state estimates always perfectly track the state, ie ~0 error. More exactly, it's about 1e-315 (yes, that's three-hundred-fifteen)
4) Also, state errors = 0 starting at t = 0; they should take some time to converge even if estimation is very good.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1319870/image.png)
But, the state estimation does seem to be doing something (what though, is unclear to me), even when L = 0. For example if I change the initial states, so that
x0 = [0.3, 0.1, 0.0, 0.0]
becomes
x0 = [0.3, 0.1, 0.1, -100]
ie the errors also start nonzero, then errors do take some time to eventually converge to 0 error.
But they will converge to 0 error even if L = [0, 0]. This seems unexpected; it's not really doing what I thought.
This plot is convergence with L = [0, 0] and nonzero initial error states:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1319880/image.png)
And this is using system poles*10, with L found the "normal" way per the attached .m file, zoomed in on the X axis.
Obviously much faster convergence, which would be expected if I thought the system was set up right, ie state estimation error wasn't 0:)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1319885/image.png)
2 Comments
Sam Chak
on 10 Mar 2023
Just wondering, why does a relatively low 5% overshoot give a relatively oscillatory damping ratio of
?
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1320175/image.png)
I don't remember the formula. Please check.
overShootPec = 5;
zeta = abs(log(overShootPec/100)) / sqrt(pi^2 * log(overShootPec/100)^2)
Accepted Answer
Paul
on 11 Mar 2023
Edited: Paul
on 11 Mar 2023
Hi John,
Disclaimer: I didn't actually look at your code.
I think all of the the results you're seeing make sense. First, let's write state equations for x and xhat (we could use x and e, but I think it might be clearer to use xhat instead of e).
State equation for the plant:
xdot = A*x + B*u (1)
State equation for the estimator:
xhatdot = A*xhat + B*u + L(y-yhat) (2)
= A*xhat + B*u + L*C*(x - xhat) (2a)
= L*C*x + (A - L*C) * xhat + B*u (2b)
Before going any further by closing the control loop, consider equations (1) and (2). If x and xhat have the same initial conditions, then we'd certainly expect x and xhat to be identical because A and B are the same in both state equations and the plant and the estimator are both excited by the exact same input u. So, with the same initial conditions, we'd certainly expect that x and xhat to be identical regardless of L. In reality this doesn't work exactly like this because model in the estimator won't exactly match the true dynamics of the plant.
Suppose instead that the plant has a nonzero initial condition and the estimator has a zero initial condition. Because LTI, the state response of the plant is the sum of the response due to the initial condition and the response due to the input. The state response of the estimator is same as the plant response due to the input becasue the estimator IC is zero. If the plant is stable (and under any stabilizing closed loop control), the plant response due to the intitial condition eventually decays to zero anyway, and when that decay is small enough the state response of the estimator will match the state response of the plant, i.e. the estimation error eventually goes to zero as the IC response of the plant decays to zero. Note that this will be true even if L = 0. However, if L=0 the "time constant" and dynamics of the estimation error is therefore the same as that of the plant, which isn't useful if we want to use xhat for feedback. Hence, we use the non-zero L feedback in the estimator to make the estimation error due to mismatched initial conditions decay to zero faster than the dynamics of the plant, and actually faster than the dynamics after applying the closed-loop control.
Closed loop control, assuming state feedback and servo tracking on x(1):
u = K*([1;0]*r - xhat)
Subbing the control into (1)
xdot = A*x + B*(K*[1;0]*r - xhat) = A*x - B*K*xhat + B*K*[1;0]*r (3)
Subbing the control into (2b)
xhatdot = L*C*x + (A-L*C)*xhat + B*K([1;0]*r - xhat)
= L*C*x + (A - L*C - B*K) *xhat + B *K *[1;0]*r (4)
Combining (3) and (4) into a single equation:
[xdot;xhatdot] = [A -B*K; L*C A-L*C-B*K]*[x;xhat] + [B*K*[1;0] ; B*K*[1;0] ] * r
Example
overShootPec = 5;
zeta = abs(log(overShootPec/100)) / sqrt(pi^2 * log(overShootPec/100)^2);
wn_rPs = 2 * pi * 40;
%% OL system
J_kgm2 = 1e-5;
b = zeta * 2 * wn_rPs * J_kgm2;
k = wn_rPs^2 * J_kgm2;
A = [0 1; -k/J_kgm2 -b/J_kgm2];
B = [0; 1/J_kgm2];
C = [1 0];
D = 0;
massSys = ss(A, B, C, D, ...
'StateName', {'theta', 'theta_dot'}, ...
'InputName', {'u'}, ...
'OutputName', {'theta'});
% control gains
ControlPoles = roots([1 1400 1e6]);
K = place(massSys.A,massSys.B,ControlPoles);
% estimator gains
EstimatorPoles = [-2000 -2500];
L = place(massSys.A.',massSys.C.',EstimatorPoles).';
% closed loop system
Acl = [A , -B*K; L*C , A-L*C-B*K];
Bcl = [B*K*[1;0] ; B*K*[1;0]];
Ccl = [1 0 0 0];
Dcl = 0;
clsys = ss(Acl,Bcl,Ccl,Dcl);
% step response
[y,t,x] = step(clsys);
figure
plot(t,y)
figure
plot(t,x(:,1:2)-x(:,3:4))
legend('pos estimation error','vel estimation error')
As expected, with the same ICs (zero), the true state and the estimated state are identical.
Comment: the plot above is not the same as what I get on my local installation using R2022a. Not sure what that's about.
Now check the response due to a non-zero IC on the plant and zero IC on estimator
[y,t,x] = initial(clsys,[1 0 0 0]);
figure
plot(t,x(:,1)-x(:,3)),grid,ylabel('pos estimation error')
figure
plot(t,x(:,2)-x(:,4)),grid,ylabel('vel estimation error')
These error responses should change depending on L. Try setting L = [0;0] and see if the step response and the inital condition responses make sense.
13 Comments
Paul
on 16 Mar 2023
As far as position goes, the integrator will drive the input to the integrator to zero for constant inputs. If the input to the integrator is r - poshat, as it was in my formulation, then in steady steady state response to constant disturbance poshat will be zero if r = 0. But as we saw, poshat = 0 doesn't mean that pos = 0.
OTOH, you said that your formulation drove pos = 0 and poshat nonzero in steady state. As far as I can see, the only way this can happen is if you feedback pos into the integrator, instead of poshat. That's a reasonable approach if you have a pos sensor (but be aware that doing so introduces sensor errors into the system in a different way than when using sensed pos just for input into the observer).
Here is my formulation using pos for feedback instead of poshat. Note that pos is used for feedback into the integrator and into the proportional feedback
xidot = r - x(1)
u = -K(1) * x(1) - K(2)*x2hat - K(3)*xi
overShootPec = 5;
zeta = abs(log(overShootPec/100)) / sqrt(pi^2 * log(overShootPec/100)^2);
wn_rPs = 2 * pi * 40;
%% OL system
J_kgm2 = 1e-5;
b = zeta * 2 * wn_rPs * J_kgm2;
k = wn_rPs^2 * J_kgm2;
A = [0 1; -k/J_kgm2 -b/J_kgm2];
B = [0; 1/J_kgm2];
C = [1 0];
D = 0;
ControlPoles = [-700+1j*700, -700-1j*700, -1000];
K = place([A zeros(2,1);-C 0],[B; -D],ControlPoles);
EstimatorPoles = [-2000 -2500];
L = place(A.',C.',EstimatorPoles).';
% feedback x(1) and xhat(2)
Acl = [...
[A zeros(2,3)] - B*[K(1) 0 0 K(2:3)] ;
[L*C , (A-L*C) , zeros(2,1)] - B*[K(1) 0 0 K(2:3)];
-[1 zeros(1,4)] ];
Bcl = [zeros(2,1); zeros(2,1); 1];
Ccl = [ [1 0], zeros(1,2), 0];
F = [B;zeros(2,1);0];
clsys = ss(Acl,[Bcl F],Ccl,0,'StateName',{'pos' 'vel' 'poshat' 'velhat' 'xi'});
format short e
zss = -clsys.A\clsys.B
Using pos for feedback instead of poshat, we now see that steady state pos = 0 in response in response to disturbance, but steady state poshat is now nonzero. This is not surprising given that pos is now the input to xidot.
t = 0:1e-6:.04;
Step responses look nearly, if not exactly, identical to the step response when using xhat(1) for feedback.
[y,t,x] = step(clsys(1,1),t);
figure
plot(t,y,t,x(:,3)),ylabel('pos, poshat')
figure
plot(t,x(:,2),t,x(:,4)),ylabel('vel, velhat')
Response to disturbance
dist = 1*(t< .02);
[y,t,x] = lsim(clsys(1,2),dist,t);
figure
plot(t,y),ylabel('pos')
yline(zss(1,2))
Now pos = 0 in steady state respons to disturbance.
figure;
plot(t,x(:,3)), ylabel('poshat')
yline(zss(3,2));
But poshat is nonzero until the disturbance is removed.
figure
plot(t,x(:,4)),ylabel('velhat')
yline(zss(4,2))
The velhat plot looks about the same as above using xhat(1) for feedback.
As far as velocity goes, when I say "expected results" I mean expected as based on the math that describes the specific implementation. Now I'm wondering, but am not sure, if you're expectations are statements of what should be feasible to achieve using some other design strategy than what we've been discussing in this thread. But, I have been showing in this thread that the steady state velocity estimate from lsim is fully consistent with expectations based on the math. If you want to be able to estimate velocity better, maybe consider implementing an observer based on an augmented plant model to estimate the disturbance, or maybe even a reduced order estimator would be helpful if willing to use sensed pos for feedback. I'm wondering if a reduced order estimator will be better at driving the velocity error to zero becasue it uses pos as an input.
More Answers (0)
See Also
Categories
Find more on Classical Control Design in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!