Control Bootcamp: LQG Example in Matlab

welcome back so now we're ready to simulate the inverted pendulum on a cart system and we're ready to stabilize it using lqr feedback and we're also going to estimate the full state using a common filter okay so I'm going to do this in in stages and I'm also going to do this using Simulink so I want to kind of draw out a few things for you so basically what we're going to do is we're going to take our real system this is whatever our system okay and our system is going to be this inverted pendulum on a cart so this is actually easier to simulate the full nonlinear system in Simulink than it is to write a script so I'm just going to simulate the whole system the idea is that we're going to have some input U and we're going to have some output X or Y or whatever so in the first case let's just say that I have a measurement of the full state X and what am I going to do with that well I'm going to pull that into some lqr regulator which is minus K X okay so that's what this block is going to do and we know that we can stabilize the inverted pendulum on a cart just using an lqr regulator okay so in a minute what we're going to do is we're going to drop this assumption that we have full access to the full state X and we're going to put a common filter estimator block in here but for now let's just make sure we can do the linear quadratic regulator and remember what we're going to be stabilizing about is the position 0 0 PI 0 okay so that's X X dot theta theta dot here X is the the position of the cart this is my vector X which is all of all of these states and a vector okay so I'm just going to show you so first things first I have my code here that builds a B QR and my calm and focus are my lqr gain matrix I'm just going run this because I have to load this all on to memory okay and then what I do is I have this nice Simulink system that I built and you can download this and play around with that I'm not going to explain every single piece but basically I have this block here cart pend sim and what this is is this is basically some C code that simulates the right-hand side the full nonlinear dynamics of the system so it's the four OD es we had before so it takes in U and it outputs the full state so I'm really measuring everything and I'm going to give that to some oscilloscopes so we're going to be able to plot the four the four states in time and then what I do is I essentially split that vector into its four components so X X dot theta theta dot and I'm going to do a couple of things so first thing I have to take theta minus PI because remember the full nonlinear system in the pendulum up position theta is equal to PI but my calm my lqr regulator is linearized about that up position so I need to look in local coordinates theta minus PI because if I'm a little bit to the right or the left I want that to measure is like point 1 or minus point 1 not PI plus point 1 or PI minus point 1 so I have to I have to kind of subtract off the equilibrium from my state of the nonlinear system and then what I'm going to do is I have this step here this is kind of cool so I have this step let me just open it I have a step at time 10 from 0 to 1 and so what that's going to do is that's going to set my equilibrium reference condition for the cart position X basically what this is going to say is that this is going to go to 1 at T equals 10 ok so the system is going to start with the pendulum up at x equals 0 at time 0 and then at time 10 it's going to say well actually I want to be at position x equals one and so the controller is going to have to walk the system over to x equals one at time 10 and then here it's a little hard to see but in Simulink this is literally just I'm multiplying minus K times the output here which is my full state X minus the equilibrium okay I know it's all of this stuff in the middle of doing this is just let me just annotate this really quickly all of this stuff here is just X minus the equilibrium okay that's all those this is doing and I want my equilibrium to change from x equals 0 to x equals 1 the cart position at time 10 so I want to show that this is actually moving the system over ok pretty simple I've got some nonlinear dynamics I measure the full state I subtract off the nominal equilibrium position I run it through my lqr gain matrix minus KX that's all this is doing and I feed that back as the input to my system okay and if nothing went wrong I should be able to run this so here I had to compile this this code so you might want to check that your compiler runs but all I have to do is hit go and it simulates it initializes it's running and it's done so now I can open my scope my oscilloscope it's a little hard to see but basically what we have here are the four variables in time and I'm just going to kind of infer what I think these variables are doing so this red variable must be theta because it's hanging out around pi okay at time 10 remember my I made my position walk from 0 to 1 so that's this yellow curve here at time 10 my controller walks my system over to position 1 and there's a little dip in theta and a little bit of theta dot and X dot right so essentially what's happening is when it walks over it probably kicks theta a little bit maybe in this direction and then walks over and stabilizes okay so we've already written the script that implements this lqr controller I'm just showing you that it's possible to do in Simulink okay that's all this is showing is in Simulink I get the same stabilization of the theta equals PI and I'm able to walk my system over let's say from cart position zero to cart position one really simple okay so that's all that this is doing so now what we're going to do is we're going to assume let's let's not assume that we measure all of X let's assume that we actually measure we only measure some limited measurements Y and so I'm going to do the following I'm going to say well okay I want to know what the truth is I'm going to have X true coming off of this thing but then what I'm going to do is I'm going to essentially I'm just going to pull up the whole Simulink window here so essentially what I'm going to do is I'm only going to measure CY okay so I'm going to measure this is going to be a little hard to to write and fit okay I'm going to measure y equals CX then what I'm going to do is I'm going to pipe this into my common filter my big common filter okay so this is y and remember my common filter needs to know you you and then the output of my common filter is an estimate of the true full state X hat which then goes into my lqr regulator okay so this is this whole procedure is the the linear quadratic Gaussian okay this is called l qg linear quadratic Gaussian it basically combines the linear quadratic regulator and the calm and filter for systems that have a an output Y that's not the full state so remember my lqr needs the full state but if I'm only measuring Y and I'm not measuring my full state I have to estimate X and then I plug it into lqr so this is a simulant code that does basically the same thing and I'm just going to walk you through it okay so this hasn't modified much over my l QR code I still have the same nonlinear dynamic I still have the same truth measurement coming out but here this is my y equal C times X so I'm only measuring and let's actually make sure that this is yeah so the gain matrix here I don't know if you can see it but it says C so the matrix here is C times X so the output is y okay this block here is my common filter so if I double click it you see that the a matrix is my common filter dot a matrix the B matrix is my common filter dot B Coleman filter dot C Kalman filter D so in the in the MATLAB window up here I'm basically going to run all of this code that builds my common filter in case I'm going to define a C matrix of D matrix and disturbance and noise covariances I'm going to build my common filter gain using either the lqe or lqr command and then I'm going to actually build a linear system called sis Kuhlman filter that is this dynamical system estimator I'm going to run all this code now in memory there's a system called sis Coleman filter and if I type it in here this KF is this big see if I can lift this it's this big common filter system okay so it has an a matrix of b matrix of c matrix and so on and so forth and then if i type this KS a it just pulls out the a matrix same with dot b c and so on and so forth okay so essentially what I'm doing is I'm taking this big block here this calm and filter block and I'm pretty sure if I wanted to I could actually name it I could call this column and filter okay and what I'm doing is I'm just defining this the ABC and D matrices of that system are sis KF my common filter okay okay and then all of this stuff looks a little weird this is just doing the same thing I'm subtracting off the equilibrium position and I'm manually putting in a step so I want to lock the cart over from position zero to position one at time 10 okay so that's just what we were doing before this is just subtracting out the equilibrium position and then is there anything else I think that's it then I run it through my so I get an estimate of my full state X hat that goes into my lqr and then that feeds back and stabilizes my system okay so let's try to run this ran super fast I double click my scope and notice again almost exactly the same response I have my my system is stabilized in the vertical up position because this theta is hanging out around pi the yellow curve which is my X position of my my cart is walking from x equals 0 to x equals 1 at time 10 and notice that this is all working purely based on a measurement only of the yellow signal so all I measuring is this yellow cart position and how I'm kicking the system and I'm estimating using this common filter theta X X dot and theta dot okay so this is really cool that we're able to build up this estimator and when we build this estimator we're able to combine it with lqr to stabilize an unstable system okay so that's the whole idea behind LQG this full design this sensor based feedback of an unstable system now if you played around with this a little more you'd find that if I added a lot of noise to this measurement or if I added a lot of disturbances to system it's actually a lot more sensitive when I combine these two okay so the combined system is more sensitive to noise and it's more sensitive to disturbances and this is a general issue with these linear quadratic gaussian controllers is that they can be kind of non robust to no lies to disturbances and so that's something we'll talk about later but I just wanted you to see how these pieces combine okay thank you

Channel: Steve Brunton

Views: 42,791

Keywords: Control, Control theory, Linear algebra, Eigenvalues, Closed-loop, Feedback, Observability, Gramian, Applied Math, Matlab, Estimation, Kalman filter, LQG, Optimal control, Inverted pendulum, Simulink

Id: reRT8LbPhBs

Length: 13min 26sec (806 seconds)

Published: Mon Feb 06 2017