How to Merge Accelerometer with GPS to Accurately Predict Position and Velocity

all right top of the morning everyone this is Scott Lobdell and this is a quick tutorial video about how you can pragmatically go about merging sensor input in particular between a GPS and an accelerometer so I'll go ahead and just write these down if I have accelerometer and I have a GPS then what we're going to do is going to take these inputs and we're going to output predicted positions and the value of this is twofold one is that I can get predicted positions at a dramatically higher frequency than I would otherwise be able to get with a raw GPS signal and then the other advantage is that using the accelerometer I can get much better GPS reading so your GPS readings are going to be on average accurate but otherwise there's a standard deviation associated with your GPS and so you can use the accelerometer to sort of correct all of those values so let me just talk briefly touch on these sensors a bit more so your accelerometer by itself is also going to be fairly useless in the context of predicting position so for example if I give you a rock cell eration so from you know sometimes here some of the time period this is my acceleration and I ask you what was my final position you would be able to tell me because you don't know my initial position was you don't know what my initial velocity was so if I can merge those with a GPS then I can give you a sort of accurate estimate of where my new position is going to be so that's sort of step one the step two is just sort of how these sensors actually complement each other so a GPS is going to be low frequency and will accuracy but the values are relative to some absolute position and then your accelerometer is actually the opposite that's going to be high frequency and high accuracy but it's going to be subject to drift and what that means is that over time it's going to accumulate errors so even the smallest amount of error is going to accumulate and then after you know even several seconds your predicted position is going to be way far off from the actual real position and so to go about doing this we're going to end up using a common filter and then the purpose this video is not to explain what a common filter is or even to explain it well because I think a lot of other resources already do that and they already do it better than I would be able to so the point of this is just to say how can I go about implementing a common filter which I had trouble with and I had to spend a lot of time researching and understanding so this is what this video is meant to do and distill so if I have a common filter then I have sort of two input steps there's predict and update and your prediction means I'm going to input in this case my accelerometer and I'm going to use that to predict my position and then the update is going to be my actual GPS and that's going to give me a if it's going to sort of take me back to a position as close to reality if my prediction step is off so that outputs your predicted position so that's sort of what the API of a common filter we look like to actually set this up to actually define this in code now there's a whole bunch of variable associate with the common filter and exact are actually pretty overwhelming but the point is if you can just understand your use case and understand how to define those variables then the actual implementation is to literally just follow the formula so then go online you can find a formula for a common filter and you can just define what the variables are then you're pretty much good to go so that's sort of the piece that you need to understand and so there are a number of variables in a common filter I'm only going to go over the ones you need to define a lot of the other variables are just defer or calculated from your inputs and so here's here's essentially what you need to define you have a number of these a BU Q R and D okay so those are the fine all of these then we can do our implementation so a is going to be your state transition matrix and what this will do is it will define what your system will look like absent any inputs so in the position and accelerated acceleration example or not even acceleration yet just position and velocity which by the way that's that's what our common filter is maintaining its position and velocity in our case that's what we're estimating okay so if I told you that I'm in space and there's no wind resistance and I'm at some position at some time and I have some velocity at some time then with a state transition matrix you will know that given over the course of several seconds or whatever I can predict exactly what my new position my new velocity is going to be okay and then be continue on with our definition to your B is going to be your control matrix so in my little space example this means that this is going to define given new inputs to system how is that going to affect position velocity and your input in this case is U which it will call that the control vector so to bring this full circle your control vector is acceleration of example your control matrix is what we multiply that acceleration by and I'll go into this in better detail later q and r are both very similar this is your your variance your covariance matrix for your two inputs so q maps to acceleration R max mass to GPS in this case so this is your process variants and this is your measurement variance so these are both covariance matrices it will end up looking something like this you know if you have a 2 by 2 matrix it's going to be your your variance variance diagonally and then 0 the rest of the way and so the way you can get this so for example by just taking accelerometer and I put it on the ground and I just leave it at rest and I measure a bunch of samples and I can compute what the standard deviation of those is and if I square the standard deviation then I can come up with a covariance matrix same with your measurement variance you do the exact same thing but it should be noted you can't what I just described you can't actually do with the GPS because your GPS would get a lock on a satellite and you would end up getting very accurate readings and so you'd have a overconfident covariance matrix so what you can do here is you can just use like the manufacturer specifications it specifies how accurate that is and you can plug that in directly here now it should also be noted that your your GPS readings will give you like a P dot value so it'll tell you how how good the satellite fixes but you can't translate this directly into into standard deviation in terms of meters you could do your own sort of heuristic to determine this but for simplicity sake you might just want to set it to some constant value on the other hand velocity readings are done using Doppler shift and so your GPS will actually tell you in terms of meters what the what the error is of your reading so you can actually in your measurement include velocity and you can redefine your covariance matrix to include that error and then finally moving on with our definitions here Z is just going to be your actual measurement vector and that will just be your raw GPS readings so you're going to get position velocity directly from the GPS so go somewhere else on the internet do some googling but if you take these variables we're going to define these and rescue the example and then the actual implementation of the common filter from there should be relatively straightforward so let's get another sheet of paper here and then we'll continue through our example so I have acceleration and I have position so now we want to be able to determine what position velocity is at any point in time and so the way that we can do is we can just pull out some physics here if you don't if you never seen this equation before maybe just do some more glue googling and sort of you know learn about this or whatever but it is also like pretty intuitive or sort of so this is just a standard physics equation to determine position of block or the equation that is true so your final position is equal to your initial position plus your velocity over the Delta time period and acceleration over your time period squared multiplied by 1/2 in the same way you can compute velocity over time and that's just going to be your initial velocity plus acceleration over a particular time period okay now we can take these two equations and we can represent them as a system of equations so I can represent this as P of V is equal to I'm going to take take this right here and I'm going to turn that into a matrix and so that will become 1 delta T 1 and 0 and if we multiply this by our previous state absent no other inputs to the system meaning absent any acceleration this is going to be a true statement so if I'm in space I can estimate my new position my new velocity by populating this matrix with my Delta time period and multiplied by the previous statement I would get my X in my updated position velocity now if we want to open over here that was basically assuming zero acceleration now if we assume input acceleration turn this into a matrix as well and we would get 1/2 delta T squared and we would get delta T multiplied by our input acceleration at this point I should point out for practicality sake I mentioned that we're merging GPS and accelerometer but I didn't mention that what we want to do is split this out into three separate dimensions so I have GPS readings that will give me a latitude longitude and altitude and then my accelerometer those need to be expressed in terms of absolute North Eastham down or at least along those axes and so this the accelerometer reading off the shelf that MIT self is going to be an entirely like entirely different problem I actually have another tutorial video that explains how to take a X Y or your acceleration vector these are three values of the ax a y AV your GX Gy these are your gyroscope reading and finally your magnetometer reading so with these three you will be able to get an acceleration in terms of north east and down and again that's an entirely separate topic that's entirely different problem as well and you can look in another tutorial video to understand how to do that if you don't already know how so the point is if I have three dimensions then I could represent all of this is a single common filter but it would get pretty complicated or it's not necessary so a simple and straightforward way of doing this is to have a separate column and filter for every single dimension so going back to here when I'm doing all of these these computations here this is assuming a single dimension so this is either going to be media I get the idea there's going to be one of three dimensions and I should mention that before going any further so I've turned this into a system of equations with matrices and then if we go back to our common filter definition we had these variables that we need to account for and so we've already done it so this PV is your state this right here is your state transition matrix so that's a previous state this matrix here is going to be B this is going to be U and let me just call go and write these out these are the variables that I wrote on the other sheet of paper define these we're good to go so we've got we've got a we've got B we've got you q and r are both the same thing they're both going to be covariance matrices I explained earlier how you can how you can come up with those so those are good those are good and I be is going to just be your actual PV measurements from the GPS okay so now we have all of those and so now all you need to do is take these matrices compute the Delta time period and then define those to find those in terms of the matrix do the matrix multiplication to compute this at every step and that's really it and the rest of everything there is reasonably straightforward hope this helps and thank you for watching [Music]

Channel: Scott Lobdell

Views: 62,764

Rating: 4.8936753 out of 5

Keywords: kalman filter, gps, accelerometer, sensor fusion

Id: 6M6wSLD-8M8

Length: 14min 13sec (853 seconds)

Published: Sun Mar 05 2017