Control Bootcamp: Kalman Filter Example in Matlab

welcome back so in the last video we looked at when this inverted pendulum on a cart is observable okay so we looked at what C matrices made this pair AC observable for this full state and also for just the state x dot theta and theta dot and so now we're actually going to develop a calm and filter to estimate this full state so all four variables from a single measurement Y and if you recall we're going to use the measurement C equals 1 0 0 0 which corresponds to just measuring the carts x position the first variable here okay that was observable so a couple of things about common filtering or estimation in general so it's actually pretty difficult to demonstrate the Kalman filter for this pendulum in the up position because that system is unstable so what happens if I start my my pendulum in the up position and I have my cart and I let it go and I'm all I'm doing is estimating so I'm not actually doing feedback control I'm just estimating well very rapidly the pendulum fall down and it leaves the region where the linearization was valid so the linearized model is only valid near the equilibrium of the pendulums up and because it's unstable the only way I can really have a good calm and filter demonstration for the pendulum in the up position is to already be stabilizing it using full state feedback okay so I'm going to wait to look at the pendulum in the up position until we're ready to combine the Kalman filter and the lqr regulator when we combine those we can actually look at the system in the pendulum up condition because our full state estimate will then be fed back into an lqr controller it will maintain the system stability and keep the system near the fixed point where this model is valid but for now we're going to look at this gantry crane configuration where the pendulum is in the down position and all we're going to be looking at is the pendulum essentially moving the cart moving left and right and the 10 swinging about in the down position okay so this is stable observable and that's how I'm going to demonstrate the Kalman filter okay and another thing that I think is important is I'm actually going to demonstrate the Kalman filter on the linearized dynamics so I'm really going to only simulate this linear dynamical system it is possible to use that and then apply it to estimate the state in the full nonlinear system but it's a little bit easier in MATLAB to show you everything on the linear system so again when we combine lqr regulator full state feedback and the column and filter estimator when we combine those then we're going to actually work with the full nonlinear system okay but for now we're going to look at the linearize dynamics about the down position so here I have my basic code I'm defining the parameters we're looking at the pendulum down solution I have my a matrix and my B matrix okay and now I'm also defining my C matrix which is my measurement I'm just going to measure the pendulum start the cart position and the MATLAB wants you to give the option of having this feed through term where the input directly affects the measurements but in our case this is just zero there's no feed through from you directly to Y okay exodia's Vero's okay so I'm going to run this code good now to build a common filter there's a few extra steps that I need we're going to essentially be building an Augmented system that essentially has disturbances and noise okay so I'm going to draw the system the following way so I'm going to have my system that's essentially X dot equals ax plus bu and so what I'm going to do is I'm going to add in disturbances what I'm going to call d4 disturbances the output of the system is going to be Y but I'm going to add noise to that measurement I'm going to add disturbances and noise to my system and then we're going to take you the input this might just be eventually I'm going to close the loop and use the estimate from my common filter for full state feedback but for now let's just say I kick the system with some UI I give it some force and I see what happens and then what our common filter is going to do is essentially our common filter is going to take this Y with noise and it's also going to take this U and our common filter is going to build an estimate of the full state X hat and this means vector X it's this whole state of position velocity position velocity of the cart and the pendulum okay so this is what our common filter is going to do and so there's a few things I have to define so first of all VD is a matrix of disturbance covariances okay and essentially what I'm going to say is that these disturbances have essentially magnitude point 1 times the identity matrix so they all enter each of these states equally 0.1 times disturbance 0.1 times to serve as point 1 point 1 if I wanted to make it so that the theta dot had a lot bigger disturbances I would just take that element the 4x4 element of this matrix and I'd make it a lot bigger so it has bigger noise covariance similarly I have to define VN the the measurement noise noise covariance and since I have a scalar measurement Y this is just a scalar covert a scalar variance for this noise term and I could make this bigger or smaller and just like in the linear quadratic regulator we had the Q and the are matrices that balance state deviations and control expenditures these are essentially my tuning knobs so if I think that my model uncertainty is high or if I have really big kicks to the system I'm going to make the VD no is covariance larger but if I think I have really nasty big sensor noise I have really noisy sensors I'm going to make this V and larger than VD and the ratio of those tells me kind of how this comb and filter is going to balance its model of the system with the measurements it's getting from Y okay okay so now what we're going to do is build this an Augmented system I'm calling this B F this is kind of the full or augmented B matrix so essentially what I'm going to have is I'm going to add plus I'm going to say plus V B times my disturbance plus 0 times noise and down here to my measurement I'm going to say plus 0 times disturbance plus the N times noise okay and so essentially I've augmented my system I'm kind of lumping all of this stuff together and I'm saying X dot is ax plus bu plus some disturbance that disturbance directly enters X dot and my measurement Y is CX in this case D is really just 0 so I'm going to actually put 0 there so my measurement is just CX it's just the cart position you doesn't enter disturbance doesn't enter but there is sensor noise okay and so notice here in this the B matrix now for my bigger system where my inputs are u D and n so u D and n our inputs for my larger system if I want to draw a box around this then this is my Augmented system then the B matrix now is B it takes it and puts you it takes some inputs D and it takes it inputs n ok and similarly my C matrix is just measuring the cart position but my new D matrix this feed through term this is essentially my D term so it's D equals 0 0 vn times my input which is you d and n ok so I can build this big augmented state space system that essentially takes in disturbances inputs and noise and then just so that I know what the truth is I'm going to have this other system full output which essentially has instead of my restricted measurements y equals CX I'm going to measure all state so I have the identity matrix for C so I measure all of my estates and essentially what I'm going to do is when I simulate my system with disturbances and noise I'm also going to simulate this truth system as full state output system so that I can compare my estimated state with the true state ok it seems like a lot it actually is kind of a pain to code this up it would almost be easier to just build this calm and filter for a real experiment where I didn't have to code all this but to show you in MATLAB I want to actually add disturbances and noise okay so I'm going to run this code hope there's no errors ok good so now I have my system with a limited measurement and I also have my true system with the full state output measurement and I've meant admite to include disturbances and noise okay so now the next part is actually pretty straightforward we're going to actually build a common filter okay and there's two ways of doing this I actually like the second way better but I'll show you both so in that lab there's a command called lqe this stands for linear quadratic estimator okay and it basically gives you a common filter but you have to give it a bunch of inputs you give it the a matrix you give it how this B matrix this disturbance enters you give it the measurement matrix C and then you give it the noise covariances so here I kind of implicitly assumed that there's no transfer function from disturbance to system it's just purely additive ok but here you can assume slightly more general disturbance and noise models and what you get out I should probably just call this instead of L let's call it KF the calming filter game and let's say okay ought to change everything down here - okay one more place okay so but essentially what this outputs is the calm and filter gain matrix that essentially the matrix that allows me to place the eigenvalues of my might estimator but what I think is a little bit cooler is that I can actually use the lqr command remember I said that observability and controllability are kind of dual formulations of the same problem so if I give my lqr command a transpose and C transpose and I act like it's a and B and I give it my disturbance and noise covariances and I act like it's Q and R what I get out are my common filter gains just using this lqr optimization so it's really cool basically lqr ALK we are solving the same fundamental equations just in some kind of a transposed notation so i just want to show you that you can kind of hijack the code from the lqr command and use that to compute this optimal column filter as long as u transpose things correctly and this is actually what I usually do I usually just use the lqr command for everything cuz I'm really comfortable with it okay so what have we done we had our a matrix our B matrix and our C matrix those were just from the linearized equations we're assuming we're only measuring the cart position and we want to estimate the full state in time I have defined some disturbance and noise covariances so i'm assuming that my system is being kicked with some disturbances and there's also sensor noise and I'm essentially building this Kuhlman filter gain matrix just like before and so what I'm the last thing I have to do this is actually a dynamical system this calman filter is its own you know X hat dot equals ax Plus be you and so on and so forth and so I have to define another state space system just for my calm and filter block so here the a matrix is a - common filter Tennessee the B matrix remember my common filter takes in inputs and I wish I had reversed the order here but it takes in and puts you and why so those go through B and KF the output of my column and filter I actually want to measure all of the states that this column and filter is estimating so I give it the a 4 by 4 identity that's going to give me the 4 state output and I call the filter has no D matrix that has no feed through term so basically I'm going to be running my system outputting this measurement Y and then I'm going to be feeding that output measurement Y and my control signal you into my common filter and the output of this common filter system is going to be my full state as measurement X hat ok so now the last thing we're going to look at is we're actually going to simulate the system in the down position and I'm going to give it nosey measurements and disturbances and I'm going to kick it and then we're going to see how well our common filter estimated all of these states ok so again this is you know I'm I'm cooking up an example to show you all of the pieces in the real world this would actually be easier if all I had was measurement Y and you in the real world for my common filter I'm going to have a model a B C and I'm going to have measurements of Y and you okay so that's all I'm actually going to have in the real world so let me just draw a little box around this in the real world I'm just going to have measurements of U and Y and I'm going to be building X hat using my model of the system all of this stuff is just for my demo okay I'm just simulating what the system is doing with disturbances and noise for the demo ok so I'm going to simulate from time equals 0.01 to 50 and increments of 0.01 then what I'm going to do is I'm going to build a disturbance signal so I'm basically going to say that my disturbance input is just some big R and and some big normally distributed random vectors same thing with my noise my nose is going to be a big random vector the same size of time I'm going to say that my actuation input you I'm going to start with just zero input and then from beetee's 100 to 120 I'm going to have a big positive impulse and from DTS 1500 to 1500 and 20 I'm going to have a big negative impulse so basically that's going to be a big positive force in the X Direction followed by a big negative force in the X direction so my cart should lurch forward and then lurch backward okay then I'm building this big Augmented input that's going to go into here and then all I have to do is simulate the system I'm going to simulate my system using this else in command with this big input that has my actual actuation my disturbance and my noise so let's just plot that okay so this whole thing runs and this is kind of cool what we see is this is time on the x-axis and the y-axis is my measurement Y and you'll see this is a big fuzzy measurement of Y that is essentially you know the cart is lurching forward and then lurching backward okay that's what we thought was going to happen but there's a ton of measurement noise on top of this so now let's actually plot what the true system is so remember I had that that full state output model that I'm running in the background so I'm giving it the same disturbance but I'm giving it zero noise so I want to know what the underlying full state is and then I'm going to plot that on top and just see what the truth looks like okay so you can see this red line here is the true measurement Y the true cart position without all that sensor noise from the full state system and so finally what we're going to do is we're going to take that measurement Y and our actuation you we both know and we're going to feed that into our common filter system and we're going to see what the X hat looks like okay so this is super simple once we have this column and filter system we know what the ABC and D matrices are for this common filter system all I have to do is simulate it where its inputs are now U and Y and i'm transposing y here just so that the the dimensions line up so i simulate my coleman filter system i get x why don't I just call this I could call this X hat I'm just going to call it X and now I'm also going to plot what the estimated state is should I not run my common filter system let's run this common filter system and my Coleman filter estimates okay and so now you can see it might be a little hard to see but I hope you can see there's this black dashed line okay so the common filter measurement why is this super noisy blue signal but because it has a model of the underlying dynamics of the system because we know something about the system and how big the disturbance is a noise are and we also know how we're kicking the system with you the Coleman filter is able to estimate this true underlying red signal by this black dashed dashed curve and so you see that the black dash curve almost perfectly lies on top of this red true underlying car position and so our Coleman filter is doing a great job of squashing that noise and really estimating the true underlying signature okay so that's very promising but remember our Coleman filter is not just supposed to be noise our measurements why we also want to know what's happening with all the variables we're not measuring X dot theta and theta dot okay so let's finally compare our common filter so now what we're going to do is we're going to plot the true x x dot theta theta dot the pollute the true signal we're going to plot that in solid lines and then we're going to plot our common filter estimates in - lines this is the last thing ok and let me move this over a little so we can see it ok and I'm gonna so I'm going to walk you through this so it's a little hard to see because the curves are so perfectly on top of each other but each of these four curves is both the true full States the actual state of the system without noise and then the dashed line which perfectly lies on top is this common filter estimate except so if i zoom in a little bit let's hope I can get zoomed in here you'll actually see that my dashed lines on these four curves almost perfectly lie on top of the true solid curves so what that means is that my common filter even though the system has disturbances and very noisy measurements and I'm actuated it I'm able to very accurately estimate X X dot the position and velocity of the cart and theta and theta dot the position and velocity of the pendulum just using a single noisy measurement of the cart position Y okay so this gives you an idea of how powerful the column and filter is even if I only measured one variable I just measured the cart position I'm able to back out the position and velocity of all of my components of my system using the column filter okay so what we're going to do next and this is actually going to be a lot more physical and intuitive we're going to take the common filter estimate in X and what we're going to do finally is we're going to use that for closed-loop feedback we're going to say u equals minus K X hat we're going to use our lqr and we're going to feed that back as an input to our system and we're going to actually simulate the full non-linear inverted pendulum on a cart so this whole part here is going to be an actual simulation of the the real nonlinear system we're going to use our Coleman filter to estimate the full state and then we're going to use that to feed back into our our lqr regulator okay that's all coming up thank you

Channel: Steve Brunton

Views: 101,592

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

Id: Lgq4R-F8SX8

Length: 22min 11sec (1331 seconds)

Published: Mon Feb 06 2017