Raspberry Pi Pico W LESSON 49: Improving IMU Performance with a Complimentary Filter

hello guys this is Paul mcarter with toptechboy dcom and we're here today with episode number 49 in our incredible new tutorial Series where you're unleashing the power of your Raspberry Pi Pico W what I will need you to do is pour yourself a nice strong glass of ice cold coffee that would be straight up black coffee poured over ice no sugar no sweeteners none needed and as you're pouring your coffee as always I want to give a shout out to our friends over at sunfounder sunfounder is actually sponsoring this most excellent series of video lessons and in this class we will be using the Kepler kit for Raspberry Pi Pico W now most of you guys probably already have your gear but if you don't look down in the description there is a link over to Amazon you can hop on over there and pick your kid up and believe me your life and my life are going to be a whole lot easier if we are working on identical Hardware but enough of this Shameless self-promotion let's jump in and talk about what I am going to teach you today and what I'm going to do is I'm going to show you my solution to the homework assignment that I gave you in lesson number 48 but I'm going to start by asking how many of you were successful if if you were successful with the homework leave a comment down below I Am Legend double chest bump and if you are not successful leave a comment down below I fold it up like a cheap Walmart lawn chair okay now if you fold it up on this one don't feel too bad this is the type of problem you might run into as you're writing your thesis for your Masters in electrical engineering from Stanford or something like that so this is getting to a pretty Advanced topic but never fear I'm going to show you how to do it I'm going to clearly explain it and it's going to make sense to you okay when you see how I do it it's really going to make sense and if you didn't think of it you'll probably think ah why did I not think of that okay let's have a sip of the old Go Juice here and let's talk about let's talk about what the homework assignment actually was what the homework assignment was was to advance to to move forward in our little project where we're creating a little widget that will measure pitch and we'll measure roll and to get better accuracy to get better performance out of our little widget so we need to kind of back up and talk about where we what we've been through over the last say three lessons Okay our first attempt at building a little widget that could measure pitch and roll we took me we took the data coming off of the accelerometers the X the Y the Z accelerometers we did a little trigonometry and we calculated the pitch and we calculated the roll and we found it was very accurate and we also found it was very Snappy very fast it responded very quickly so it was fast and it was accurate but what we also found is is that it would respond to vibration or it would respond to acceleration deceleration and that would lead to an error signal in our indicated values and so we incorporated a noise error onto our device and so it was fast it was accurate but it had noise in the data then what did we do okay we want to get rid of the noise the next attempt what we did is we incorporated a low pass filter where we filtered out those rapid changes coming from the acceler accelerometer in response to vibration or acceleration and we filtered those out now it was very accurate and it was very low noise we got rid of the noise but it became a little sluggish because the more we filtered that noise out the more sluggish the response became and so our first attempt was fast and it was accurate but it had noise our second attempt was accurate and no noise but it became sluggish all right so then we tried a third approach and that was instead of using the accelerometers we use the three Gyros the x-axis y axis and z-axis Gyros the Gyros measure rotational velocity around the three axises and so we said well the new position the new angle is equal to the old angle plus the rotational velocity multiplied by the increment of in time since our last me measurement and so we could sit there and just track and kind of integrate over that rotational velocity or over those rotational velocities and what did we get we got something that was really low noise and we got something that was very responsive and fast but then we saw there was a drift problem in there that just sitting still Those sensors would start drifting and the readings would start Rift drifting so we were fast we were low noise but we weren't accurate so then what is the solution the solution is we want the Best of Both Worlds we want to take what we like about the accelerometer which is it's fast and it's accurate and we want to combine it with what we like about the gyro which is is very low noise and we want the be Best of Both Worlds because the three things we tried we could always get two out of three right we could get two out of three the three things that we wanted was fast accurate and low noise fast accurate low noise and with each different approach we could always get two out of the three well there was that old sign two out of three ain't bad but for engineers are we happy with two out of three no we're not happy out of two with two out of three we want three out of three so what are we going to do we're going to do a little bit of math don't worry I'll explain it to you we're going to do a little bit of math and we're going to create a complimentary filter that means the accelerometer and Gyro are going to complement each other and come up with the ultimate solution which is fast accurate and low noise okay sound good I hope it does there has been way too much talking and not nearly enough engineering going on here so what we want to do is I don't want to completely write this code from scratch so I want to start where we left off last week right last week we had a program that calculated roll and Pitch based on the based on the measurements coming from the Gyros just let's just get that so we'll have a starting place so I will need you to go to the most excellent www.top techboy do.com and then I will need you to use this happy little search icon here and search on something like measuring roll pitch and yaw using the 3axis gyro on the mpu6050 you'll come here this is the schematic of how we have it hooked up we've got the 1306 display we're not using it today but we've got it in the circuit cuz we will be using it in future lessons in the mpu 6050 you can see how we have it hooked up we'll come over here right Mouse click and copy and then we're going to come over to th we're going to come over to th and we are going to paste that in and now just to make sure that the universe is in proper working order we're going to run this thing and what do we expect since this is gyro what we expect is we expect very low noise and we expect very fast operation but then we expect an accuracy problem because we're going to see a drift problem but let's just go ahead and I think I should show you this view so you can see let's just make sure that it's working okay so we'll come over here now and we'll run this and then I am going to yah and you can see the yah is green it's fast there's no noise I'm going to roll and in blue it's fast and there's no noise and then I'm going to pitch and it's what it's fast and there's no noise but if I just leave it what is the problem look at the orange look at the pitch it's drifting this is not moving but the orange says it is it's it says it's moving at about a degree per second and that is leading to a large error okay fast no noise but not accurate all right so now how do we fix this thing well let's come in here let's kill the program okay first thing I want to do is you can see that we are measuring the gyro the X the Y the Z and the gyro and then we're calculating roll and Pitch well now we got to realize that this we we've got to be more specific this is the roll from the jrow so I'm going to call it roll G is equal to roll G Plus plus the uh plus the Y gyro okay the Y gyro that is the angular uh the angular velocity times the time so velocity times time that's how much we've incremented and then we add that to where we were before and similarly I'm going to call this pitch G so the new pitch value is equal to the old pitch Value Plus the rotational velocity times how long it was from the last measurement and so with with that you're just kind of integrating over the velocity to track position pretty cool right okay remember that since we're using the old value we had to initialize those variables so instead of roll up here I'll call it roll G instead of pitch up here I'll call it pitch G like that and so that is pretty good that's that's got that cleaned up also we are not able to measure ya with the accelerometers and so I can't get a ya value with the with the accelerometers so I can't create this complimentary filter for y'all we can only do pitch and we can do yah okay so I'm going to take that out and since I took that out we better not try to print it either so I'll take that out so let's kind of clean that stuff up now we want to get the data from the accelerometer and the easiest thing would be to copy and paste these statements that are reading the gyos and then instead of X gyro I'm going to call it X Excel and then I'm going to call it y Excel and then I'm going to call it Z Excel and then I'm going to be reading what I'm going to be reading the excel in the X I'm going to be reading the excel in the Y and then I'm going to be reading the excel in the Z okay then I calculate the rle from the gyro and the pitch from the gyro now I want to calculate the roll from the accelerometer and the pitch from the accelerometer this is not rocket science how do you do it I take those acceleration vectors I do a little trigonometry explained this two lessons ago so I'm not going to spend a lot of time explaining it but I'm going to do something that you should really very very clearly understand right you should really very very clearly understand that the role in the uh the role uh from the accelerometer is going to be equal to the math. arc tangent a tan and then what the value from the X accelerometer divided by the Z accelerometer that is going to give us a result in radians so we need to convert it to degrees by dividing by two and dividing by math. pi and then multiplying by 360 again I explained all this a couple of lessons ago and just like we did roll pitch is going to be very similar so then I am going to say pitch from the accelerometer get rid of this extraneous nonsense is this time instead of X it's going to be Y and now I should have roll from the accelerometer and Pitch from the accelerometer all right so let's come down here and let's see if we can we can and uh plot these this R should now be R from the gyro and this P for pitch should be pitch from the gyro and then this is going to be roll from the gyro and this is going to be pitch from the gyro and then I think if I copy this and paste it right here like that I can then say this is going to be roll from the accelerometer and roll from the accelerometer and then this is going to be pitch from the accelerometer and then this is going to be pitch from the accelerometer now what this should do is this should just kind of combine the stuff that we did in the last few lessons what it looks like when you get roll from the Gyros versus what it looks like when you get it from the accelerometers and so let's see if I can come up here and print this okay let's see if I can come up here and plot this and so we're going to run it okay and then when I roll okay I Y okay when I roll look at one thing that happens when roll they're they're right but they're opposite from each other so they using a different sign convention so the accelerometer is saying like the left low is the positive direction and the gyro is saying the left up is the positive direction so we've got to kind of get those two things to match you could do it whichever way you wanted but I think what I'm going to do is I'm going to come up let's see I'm going to come up here and I'm just going to flip the sign on which one of those would that be that was Roll and Roll was Y and so on the gyro I'm just going to say take the negative of what you think and that way you'll be in sync with your neighbor the accelerometer okay you'll be in sync with your neighbor the accelerometer so now if I run these things it should be the same so I'm going to come here and then you see when I roll the green and the blue are the same that's good and then when I pitch the red and the orange are the same but what do you see is the problem the red and the orange are drifting apart the red and the orange are drifting apart now it is the orange that is the correct value the Orange is the correct value because it is coming from the accelerometer but then what you see is the problem is if I shake it give it a little shaky shake there the orange has noise in it and the red doesn't okay now you see they move together they move together what do I like about the red it moves very accurately and it doesn't have any noise but it just wanders off it just wanders off so what do I want I want that lack of noise and that fast response that lack of noise what is that that's pitch okay that lack of noise and that fast response that the red is giving me but I want to combine that with the rock steady anchor accuracy of the data coming from the accelerometer does that make sense there's roll you see on Roll they've even now the rolls have D drifted apart between the Green between the green and the blue and then uh let's see you can see the error is in the blue which is the accelerometer okay the noise is from the blue which is the accelerometer you see that jyo doesn't have any noise but the problem is the Gyro's giving you the wrong answer so you see you see what the problem is so what do we want to do we want what we want the Best of Both Worlds now when do we trust the accelerometer over time it always gives you the right answer you just can't trust it in in the short term because it might have vibration it might be giving you noise so what do we want we want in the short term to trust the gyro because it doesn't have noise in the long term we want to trust the accelerometer because it always ends up at the right answer so how would we do that it's a little bit like a low pass filter like you could think of high pass filter the the gyro and low pass filter the accelerometer and combine those two together but then it's like you're taking this and you're taking this but you're not really fusing them together you're not really melding them together and so this is what I want you to see this is what we're going to do now we're just going to have like a roll complimentary and a pitch complimentary roll comp pitch comp and what that means is putting a complimentary filter on the roll and putting a complimentary filter on the pitch and if we're going to do that we're going to come up here and remember we're going to have to initialize those things because it is one of those things that you're incrementing onto the old value so you have to give it an initial value roll the complimentary filter is equal to zero and then pitch from the complimentary filter is equal to zero all right now now we're going to come down here the moment of truth so what are we going to say we're going to let's do the roll from the complimentary filter well first of all what are we going to want first of all what are we going to want we're going to want the roll from the accelerometer we're going to want the roll from the accelerometer but why do I like that it's the right answer but do I trust it in the short term no I don't because it might be Fe me noise so I'm going to give it a confidence of what times .1 I will trust it to 10% I will trust it to 10% now when I'm over time trusting at 10% 10% 10% it will always end up at the right answer but if it gives me some crazy signal I'm gonna kind of ignore it I'm going to listen to who the gyro so I need to add to that well if I trust the accelerometer 10% I want to trust the gyro what 90% so I'll put .9 times okay now what you might say is okay just take the gyro up here which is the old gyo plus the rotational velocity times the time and just like I put acceleration here I should just put roll G here but no that would be taking accelerometer data waiting it gyro data and waiting it and adding them together but that's not blending them together so that the magic happens the magic happens where here instead of creating a roll G based on the old roll G I'm going to create it based on the old Blended data the Blended data so do we put r g here no we put roll comp because that is the Blended data that last data point had the best of both worlds so I want to take this and I want to do the increment based on the starting point of the Best of Both Worlds you see this is not the best of both worlds this is so I'm going to take that and then I'm going to add to it what the uh I'm going to add to it what did we call that it was the uh it was the the Y gyro okay the new measurement the Y gyro this y gyro it's not the best of both worlds right but it is getting the good thing about the gyro so we're going to take y gyro and then we're going to multiply it by how long was it there well since the last measurement which is tloop like that okay so I take where I was before and I add the amount I move based on the rotational velocity times the loop time and then I'm going to trust that 90% because I've got the best of both worlds here but then I always come back and I anchor things to the value from the accelerometer okay now it's going to be very similar for pitch and so I'm going to come here and then I'm going to say the pitch compensation is equal to the pitch from the accelerometer and then it's going to be the best of both World pitch which is the pitch complimentary filter plus what this time I do believe this is going to be the xjo like that okay now we're still going to print the roll in the pit from the accelerometer that's just the raw data that's accurate but noisy and then here I'm going to put the Roll with the complimentary filter and the pitch with the complimentary filter and then this is going to be the roll with the comp the roll with the complimentary filter and the pitch with the complimentary filter okay now now when we run this what do we expect do we expect to see any Drift We do not expect to see any drift why we've got our anchor here we're going to always be anchoring our answer to The Rock The Rock is the calculation from the accelerometer okay and then to that we're going to change it by this compilation which is the result which is the best of both worlds but everything is always anchored to the Rock could this crazy thing really work what do we expect we well we're we're going to see no we're showing two things we're well we're showing four things we're showing the results from the accelerometer and we're showing the results from the complimentary filter and so the accelerometer we expect to be fast and accurate but when I shake it you're going to see an error and we don't expect to see any drift because the accelerometer doesn't have drift and on the complimentary filter one I'm anchoring it to the acceleration so this crazy thing this is so crazy it's so crazy it just might work let's run it okay and it gets started there so let me give it a value okay now let's look let's start by Rolling okay I'm going to roll and look at that look at that both the accelerometer both the value from the acceleration the accelerometer and the value of the complimentary filter they are both the same okay now the complimentary filter is it drifting no it is not drifting is it accurate yes it is accurate what is the question and is it fast yes these things are responding fast you see they're not sluggish at all but now what about noise H let's see yeah okay now look at this they both have noise they shouldn't both have noise why do they both have noise okay what you can see is the blue is a lot more noise the blue is a lot more noise than the green and then same thing with pitch it all looks good okay and then when I shake it I've got more pitch in the accelerometer and I H I have more noise in the accelerometer and less noise with the complimentary filter so that looked pretty good it was fast it was accurate there was no Drift But we did have the noise problem all right well why do we have the noise problem because I'm leaning on the Rock too heavily and I'm not leaning heavily enough on that that short term where that noise comes in I should be leaning more on this one okay I should be leaning more on this one and less less on this one so let's say instead of 0.1 let's say what if I said 0.05 okay it's still there my rock is still there but I'm going to be giving more weight to this combined value okay so now let's try that okay so I wrote that's good and now look at that you see how the green has much less noise than the blue the green the noise went way down but there's still a little bit more noise than I want so I'm going to say 0.01 and I'm going to say 99 and here 0.01 and 99 all right the rock is still going to always get me to the right answer but in all that shaking I'm be looking at the the gyro more okay let's try it again okay so I'm going to come up you see how it's still very Snappy it's very quick and now look at that boom did you see that the accelerometer the blue one it has the noise but now what the green one the complimentary filter it has virtually indetectable noise but is it still fast yes you see I turn it up I turn it up and the complimentary filter one the green uh the the the uh green the complimentary filter it is tracking the blue the green is tracking the blue quickly it's dead accurate they're both the same and then I've gotten rid of the noise success Victory guys is this is this amazing is this amazing or is am I just an old man who gets uh who gets excited about silly things let me see if I can show you that better okay there it is okay you see lot of noise in the blue no noise in the green and then let's see in the pitch in the pitch Direction Let's see if I can do it there's the pitch and same thing lot of noise in the orange and no no noise in the red now you see a little noise but you see it's because I'm not keeping it at exactly the same angle as I move it so that little bit of wavering in the red is from that and so I come here I come here I come here I come here no noise fast and dead accurate the power of math and the power of the complimentary filter okay so let's see this is the perfect solution right the perfect solution but I'm always picky so let's see if there's something I don't like I'm going to stop it and I'm going to run it without it autoscaling uh you know so I can really look down here very closely at the details and let's see there it auto scal so what should I be comparing I want to compare roll which is uh roll from the accelerometer which is blue okay so do you see this blue value that is the actual value and then the green value is from the filter so what's the good news here the green value from the filter even at a very zoomed in scale it is matching what the real number is from the accelerometer just taking the noise out but what do we see Happening Here the Orange is for the roll uh the Orange is uh the orange I should be cons yeah the Orange is the pitch from the accelerometer and I need to compare that to the compensated pitch so I should be comparing orange to red and what we see is they track okay they track but if we if we Zoom really really really far in when this thing is sitting here when this thing is sitting here there is kind of like this little bit of error now you wouldn't see it when I did this okay you wouldn't see it when I did this when I do that you hardly even see it okay but what is happening there's a little Gap there is a little gap between those two now why is there a little gap between those two there's a little gap between those two because this is my rock but I am looking so much at this one there's a tendency for the rock to be fighting the drift and they come to something they come to something where they settle to a constant value but it is uh let's see what would we call this this is not a drift between the orange and the red this is kind of like a steady state tiny error a very tiny error of maybe one degree between here and here okay are we happy with with that no we're not happy with that and I'll tell you the way you might try to deal with that let's say that I went back to 0. n and then I went back here to 0.1 and say give us more anchor give us more rock and that'll go away well let's see again we're looking at the red and the orange okay and actually it didn't take it away did it it didn't take it away so let's let's go back to point I did the wrong one never mind I I should be looking at pitch and so this one you might say it'll take it away if I do it on Pitch so I'll make this point one so you want more anchor you want more rock and you want less of this highspeed business so now does that bring the red and the orange together yeah that kind of brought the red and the orange together but now look what happens I pitch okay things look good but now I start seeing okay I start seeing in the red I start seeing some noise and so then let's say I go to 0 05 and 95 and again we're going to be comparing red and orange okay let's let it okay that look looks like we took most of it out okay and then I lean let's do it like this okay but you can see still I've got some error I've still got some error in the red and so what I'm going to do then is I'm going to come back to 0.01 and then I'm going to go to 99 like that that okay and then what am I seeing I'm still seeing some error between the red and the orange okay so you see now I've gotten rid of the drift I've gotten rid of the noise my measurement is fast and my measurement is accurate but I can end up with this steady state offset of an error of maybe a degree or so I want to get rid of that okay and so that is that is your homework assignment for next week you got to stay fast you got to stay accurate and you have to stay low noise but You' got to get that error that steady state error out of your pitch now it looks smaller here right it looks that steady state error looks smaller here just because the scale has changed right just because the scale has changed but you can see there's a little eror now we know we have more drift and Pitch but if we zoomed in on the roll we would see that as well so what we want to get rid of that steady state error that's your homework assignment for next week and then that will be the grand finale that will be finally everything brought together why have I spent so much time on this lesson why have I spent so many lessons on this topic because you need to know how to use a low pass filter okay for any project you need to know how to use a low pass filter you learn that and you also in the real world the data is not perfect and how do you deal with imperfect data and you think oh well you average it no averaging doesn't work you can use low pass filters you can use high pass filters and you can use complimentary filters for this particular problem it required a what it required a complimentary filter which is kind of like meld the low pass and the high pass together in a way that the magic happened okay but now we've ended up with a little steady state error and what do I need to do I need to force that steady state error to zero okay I need to force that steady state error to zero and then I'll show you how to do that next week and even if you don't care about even if you don't care about uh accelerometers and Gyros and tilt you would run into the exact same problem if you were doing a uh P motor controller a speed controller on a motor I did a project once where I had an electromagnet and I had a steel ball and that steel ball was suspended in midair underneath the magnet so it just hung there because I had a very sophisticated system that would pulse the magnet to keep the ball suspended in midair exact same thing you had to force that steady state error out so the techniques that I'm showing you I'm displaying them to you you know in the in the context of an mpu tilt meter but these concepts are applicable to a broad range of engineering problems and so I'm hoping you'll take these and I'm hoping you'll put them in your toolbox and you'll use them when you run into such problems in the future okay guys I really hope you're having as much fun taking these classes as I am making them as always I want to give a shout out to you guys who are standing with me on patreon you guys are keeping me in the game because YouTube has decided they don't like grumpy old guys and they want 15c shorts or silly cat videos and so they're kind of pushing us to the side you guys on patreon you're the ones keeping me uh continuing to make these videos and I thank you for it you guys can also help me by giving me a thumbs up also helps if you leave a comment down below if you haven't already subscribed to the channel and when you do make sure that you ring that Bell so you'll get notified When Future lesson drop lessons drop and finally and most importantly share this video with other people because the world needs more people doing engineering and fewer people sitting around watching silly cat videos Paul mcarter with toptechboy do.com I will talk to you guys later [Music]

Channel: Paul McWhorter

Views: 3,167

Keywords: STEM, LiveStream, TopTechBoy

Id: CFuEokTJn5s

Length: 38min 1sec (2281 seconds)

Published: Tue Dec 26 2023