Well, here is my feeble attempt at a median filter. It is an eleven point filter and it use the pasmshellsort routine to find the median value. It reads an ADXL345 with Jason Dorie's 9DOF object and uses P.S.T. to display the values. The loop runs at 250Hz. Feel free to critique it, I would appreciate it. It is very slow.
I am starting to understand the complementary filter a little better, I think.
My values of .999 gyro percent and my .001 acc percent was the reason for my considerable lag. By placing hard values in my flight code for the ACC I was able to determine how long my comp filter took to converge. I think converge is the right word.
I changed my percentages so that the ACC angle and the Gyro angle converged in about 5 seconds. My flight was very unstable.:frown: So now I am thinking that I am getting too much noise in my ACC, I still have not figured out how to filter it properly.
And yet another DCM question. The DCM from what I understand is a comp filter, but it handles angle estimation and orientation better than a simple comp filter. My question is, does the DCM equation allow for the gyro and acc data to converge faster?
OK last question, which low pass filter is better for filtering raw ACC data?
This one
aY := aYLFS + ((AlphaACC * (aY - aYLFS))
aYLFS := aY
or this one.
aX := (aX * .98) + (aXRaw * .02)
The second one is a complementary filter.
Or are the results comparable, just two different ways to write a low pass filter?
Answering your second question first, those formulas are almost exactly the same.
Rename some variables and it looks like this: AY := AY + (0.02 * (RawAY - AY))
Expanding that out gives you: AY := AY + (0.02 * -AY) + (0.02 * RawAY)
And then simplifying that gives you: AY := (0.98 * AY) + (RawAY * 0.02)
Look familiar? (AY + (0.02 * -AY)) is the same as (0.98 * AY)
Convergence rate is going to be basically the same regardless of what type of orientation algorithm you're using. If you have the same data being fed in, and you're using the same scales, one is no more magical than the other. I'm having basically the same problem as you right now in that my sensor readings are too noisy, meaning I can't reliably use the accelerometer as an orientation reference. No amount of filtering is going to turn random noise into meaningful data.
If you have your sensors "hard mounted" on your frame, unless your props are *perfectly* balanced (and even then, really) you're going to have noise issues. You need to mechanically isolate the board from the quad and dampen as much vibration as possible *before* it gets to the sensors. I'm thinking of mounting my board to an 1/8th inch aluminum plate (for mass) and then using moongel to stick the plate to the quad, along with some rubber bands or something to make sure it stays put. Also thinking of just using some small pieces of Zeal on the corners of the plate. Using very small pieces of whatever material you're using means you don't need as much mass sitting on it for the damping to be effective.
There are lots of ways to do it, but *anything* is better than nothing. You want to get the sensor vibration as low as you possibly can. It'll mean you have to do less filtering, and your results should be significantly more accurate.
That was an interesting video Jason, thanks.
Those links were good also, I already had the top one bookmarked. I was(am) going to try the double sided tape on my new frame. I think I am also going to rewrite my IMU code 1 more time before I switch frames and motors. My bench top tests with the median filter look promising. I want to see how they hold up in a real flight with motors vibrating.
Have you played with a median filter on your quad? My version of it is kind of slow, it takes about 1.5 mS to run my 3 values through an 11 point filter.
My thought process right now is that I am going to code this thing and hopefully have my flight loop running at 200Hz still. My code will need refined and I will have to learn how to code better, but in the back of my mind I am thinking that if I can keep it at 200Hz or maybe even 100Hz when the Prop 2 comes out that will double my speed. I know the prop 2 isn't coming out for awhile but it will take me that long probably to learn everything. Also if Bean and Batang come out with the new Prop Basic Studio that might be a way to speed it up. The best thing for me to do is to get better with coding and learn more Trig. I think I read that the new prop will have native multiplication and divide, that will be sweet if i read that right.
Shawn
The new Prop will be more than double. The clock rate is doubled (160MHz instead of 80) but it also runs 1 instruction per clock, instead of 1 for every 4 clocks, if I'm not mistaken. So, if I'm right, it'll be an 8x speedup. Also hub access is significantly faster too, which is one of the reasons Spin is slow, so that should help a lot too.
Your median filter would be better done in PASM on the cog that's doing the sensor reads. It wouldn't be trivial code to write, but it'd go like a scalded cat. I haven't tried a median filter yet (not for this stuff) but it's on my list of things to try, for sure. You could boost your speed by reducing the number of points you're sorting - Try 6 or 7 instead of 11. Should go twice as fast, though it won't produce as much of an effect.
I have a couple curiosity questions. I have been reading the forums now for months and I am just wondering how many people have actually gotten a quad auto-leveling satisfactorily. I know Jason has and and A.Eser has. I have read a ton of threads where people have said they were building one and then the thread just dies with no follow up.
Jason, did you go from gyro only to DCM or did you try a kalman filter or complementary filter in between.
Just curious is all.
What do you guys think of this idea? The gyro seems to have the most problems when the pitch and roll axis are not level and yaw is applied. So what I am proposing is that my multipliers for my complimentary filter are scaled by a percent depending on how much yaw I am applying. I may have not explained that very well.
I have the most problems with returning my quad to level after I have done something like this. Pitch forward 35 degrees and apply yaw heavily at the same time, if I let off the pitch and yaw completely all hell breaks loose. I maintain attitude pretty good with mulipliers of .999 and .001 for my comp filter no matter what I do as long as I do not yaw heavily, actually I can yaw heavilly as long as my quad is reasonably level. So then when I apply yaw I will scale those two values from lets say .999 and .001 to .996 and .004 depending on how much yaw I have applied.
I don't think I have as much of a problem with noise as I thought coming from my ACC. I actually think it is more of a problem not understanding the comp filter, which I think I understand now. I have been sitting here rotating my board in all kinds of directions making note of how quickly it responds to changes in orientation. If I put too much ACC in the equation I think it is killing part of my Gyro reading, which causes a lag in response. And if I put too much gyro it adds a huge lag in its response to yaw forces on the board.
None of that probably made sense, its a good thing I am not a teacher.
I am sure no one is following my train of thought from my last post, but I have morphed my thought process further. I am thinking instead of scaling my comp values based on my yaw input to something different. I am thinking that I will run my acc and gyro values though some sort of PID loop, maybe just a P loop. When my acc and gyro angle values get to far apart, the loop will change my comp percentages so that the 2 values converge faster. And when my gyro and acc angles are close it will change my comp percentages so that the 2 converge slower this way my gyro would play more of a roll for maintaining attitude and my acc would play more of a role when I am flying and adding yaw. I would determine a max and min percent value for my comp filter and clamp the loop at those values. By doing all of this I think I can control how fast the 2 values converge in different situations.
Am I off my rocker? Or has anyone tried to do this before? I have not found an example of anything like this. No one probably has a clue what I am talking about because my brain just exploded all over the screen in an unorganized manner.
Well after playing with the complementary filter some more I think I fully understand how it works and its limitations. I have not finished my third version of my flight controller but I am going to start to look into the DCM option. I am thinking that convergence is how fast the values from the gyro and the acc come together when they are not equal, I may be wrong there.
I am going to finish my third version of the flight controller, however I am not anticipating great results. Altering the alpha value of my comp filter seems to be working while I move the board by hand but I am not sure how it will work in a real application.
From what I have been reading no matter what fusion algorithm is used they all have problems with centripetal force. I would assume this has to do purely with the sensors capabilities. After research and experimenting I am pretty sure my main problem is the fact the gyro gets lost when an axis is tilted and yaw is applied. With a varying alpha I have been able to reduce the lag in this situation. I think my gyro is solid, I am using a alpha of .9995 during attitude hold and a down to .9980 during extreme flight. Like I said I have not actually tried the new flight program yet, but I did use the .998 hard coded as alpha in my last version and it flew but had a hard time at maintaining attitude.
DCM here I come.
I think that a comp filter will work fine for just maintaining attitude, but to change from flight to maintaining attitude it might be lacking. Hopefully I will have my third version running by the forth. Initially I am going to leave the median filter out.
Mini test one is successful. By varying the alpha of my complimentary filter for roll and pitch during forward flight or side ways flight with yaw, my quad seems to return to level attitude if I let off of the sticks. It definitely needs some fine tuning but the initial results are looking promising. If my acc angle and my gyro angle are off by more than 10 degrees I change my alpha from .9995 to .9800 which allows the two to converge faster. I am definitely getting excited about DCM. Hopefully with DCM the angle estimation does not require this kind of manipulation.
I took the quad out this morning, the test did not go well. The weather was perfect for flying though, no wind.
This is a quote from a link I posted in post #50.
So, the scenario of concern is when then the normal vector of the quadrotor isn't vertical. Then, rotating about that normal axis (yaw) will cause the pitch and roll angles to change in a way that isn't measurable by the X and Y rate gyros. This animation shows yaw with and without the normal axis vertical. The accelerometers will still pick up the changing pitch and roll angles, but they are low-pass filtered in the complementary filter so they will only update slowly. Rapid yaw while not horizontal could result in bad angle estimates, and consequently a loss of control.
I am trying to decide which way to go next. Try to implement the formula from this article to deal with the yaw problem or switch over to DCM. http://scolton.blogspot.com/2012/09/fun-with-complementary-filter-multiwii.html
If DCM still uses a comp filter to fuse the acc and gyro data I would imagine that the yaw problem still exists since the acc is fed into the final angle slowly.
I am curious how others are dealing with this problem. If the DCM proccess has the same limitations I think that this link may be helpful to those of you using DCM also.
I attempted to figure out the equations from that link before, but could not grasp it, my math understanding is a little better now so I think I may try again.
The DCM doesn't suffer from this orientation (yaw) problem because it deals with the orientation of the quad as a whole, not as discrete axis. That is what's causing your issue - You assume that when you yaw you're always yawing around a line that is perfectly vertical, and that's not true. As soon as the quad is tilted away from being perfectly horizontal, yaw affect more than just your heading. The DCM code constantly tracks the complete orientation of the sensors, and makes incremental updates to them using the gyro and the acc.
Try this - Start with your quad in front of you, pointing forward. Turn your quad upside down by rolling it 180 degrees. Now change the heading by rotating left 90 degrees. Now roll it back to upright again. Now, where does your quad think it's pointing? Because it was upside down, rotating it to your left is actually rotating to your quad's right. The sensors you get your readings from are attached to the quad, so they rotate with it. So when your quad is upside down, your sensors are upside down too, and the readings aren't aligned with the world any more. Your quad thinks it's pointing to your right. Where it believes itself to be is 180 degrees away from where you believe it should be.
This is an extreme example, but it's exactly the problem that DCM and quaternion based IMU systems deal with. You take your current orientation (quaternion or matrix) and update it using readings from your gyro and accelerometer. Since those readings happen frequently, the changes to the orientation are relatively small. You then figure out where the ground "should be" based on this orientation value, and you compare that to the acc reading. You figure out the difference, and apply some amount of that as an additional rotation to re-align the orientation with the acc. (this is the complimentary filter part). All IMU's that use an accelerometer are going to have issues with centrifugal force, but that's not the same problem as tracking the rotating orientation frame.
Start reading up on rotation matrices - If you can wrap your head around the basics of those, the DCM code isn't that hard. Pay particular attention to this bit:
One can interpret the row vectors of a rotation matrix as unit vectors along the axes of the rotated space, in the original coordinate system. Likewise, the column vectors represent unit vectors along the axes of the original space, in the rotated coordinate system.
The DCM doesn't suffer from this orientation (yaw) problem because it deals with the orientation of the quad as a whole, not as discrete axis.
Jason, that pretty much determines where I will go from here. DCM here I come!
I am glad that I have worked my way through this comp filter stuff with your help. I am finally getting a pretty good idea of how things work and what I need to do, to make my quad fly. Don't get me wrong, I am no where close to having a quad that flies well, but pushing my way through this has taught me a lot about all sorts of things.
Right now I just want to fly something, I am kind of at that point where I want to fly instead of build. I have been playing with the equations from the link that I posted earlier today. I am actually getting the math to work now, but it is all in floating point. It seems kind of slick how it works, my last attempt with the comp filter will be to try and apply the yaw compensation from that post. Hopefully I can get something that is enjoyable to fly and not just hover. No matter what the outcome, I am going to jump into DCM next.
If you completely ignore the part where the quad tries to make itself level, and just try to make it stable, all you need is a gyro and it's quite stable and flyable. That's what the first version of my QuadX code does. I keep one of those around so I have something to fly while I'm working on the other stuff, just because it's fun to take it out and rip around once in a while.
The gyro only makes sense, but to be honest I am not that good at flying, auto level is a nice safety net when I loose orientation. I can see why people that are real good at flying use gyro only, they can push the limit more with out the flight controller lying to them. I want to eventually do FPV and shoot video and still shots. Maybe even make it fly itself, but I am not that far. Just attempting to program a board and testing it has made me a better pilot, although that's probably not the best way to learn how to fly.
I have finished the coding on my yaw compensated complimentary filter(actually its not mine). I took it outside for a test hover, but I didn't have time to go to the park and crank on it. http://scolton.blogspot.com/2012/09/...-multiwii.html This is the link I found the code at. I know I posted the link just a few posts ago, but I do not like to take credit for other peoples work.
I was amazed at how well it hovered, I would have to say it out performed the plain complimentary filter. Tomorrow I am going to take it to the park if it is not windy, and test out how well it works for yaw compensation. On the bench it works perfectly. But I also said that about the code I wrote last night. Mr Coltons code is way more advanced than mine. Here is a link for the yaw compensated comp filter. I wrote a short demo to see how it responded. The IMU I used was a sparkfun 6DOF board.
Depending on how well my flight goes tomorrow this maybe the end of my journey with the comp filter, at least for this project. If all goes well I will post a video, my next task is to dive into DCM.
Well, I had my wind free test flight with the yaw compensated complementary filter this morning at the park. The quad flew really nicely, and way better than the plain comp filter.
Now I have something to fly while I am learning DCM. I will post a video as soon as I have time.
Just for the hell of it I am going to slow my flight loop down to 50Hz just to see how it handles. I will post a video of that also.
Comments
11 Point Median Filter for ADXL345.zip
Shawn
I am starting to understand the complementary filter a little better, I think.
My values of .999 gyro percent and my .001 acc percent was the reason for my considerable lag. By placing hard values in my flight code for the ACC I was able to determine how long my comp filter took to converge. I think converge is the right word.
I changed my percentages so that the ACC angle and the Gyro angle converged in about 5 seconds. My flight was very unstable.:frown: So now I am thinking that I am getting too much noise in my ACC, I still have not figured out how to filter it properly.
And yet another DCM question. The DCM from what I understand is a comp filter, but it handles angle estimation and orientation better than a simple comp filter. My question is, does the DCM equation allow for the gyro and acc data to converge faster?
OK last question, which low pass filter is better for filtering raw ACC data?
This one
aY := aYLFS + ((AlphaACC * (aY - aYLFS))
aYLFS := aY
or this one.
aX := (aX * .98) + (aXRaw * .02)
The second one is a complementary filter.
Or are the results comparable, just two different ways to write a low pass filter?
Thanks
Shawn
Rename some variables and it looks like this: AY := AY + (0.02 * (RawAY - AY))
Expanding that out gives you: AY := AY + (0.02 * -AY) + (0.02 * RawAY)
And then simplifying that gives you: AY := (0.98 * AY) + (RawAY * 0.02)
Look familiar? (AY + (0.02 * -AY)) is the same as (0.98 * AY)
Convergence rate is going to be basically the same regardless of what type of orientation algorithm you're using. If you have the same data being fed in, and you're using the same scales, one is no more magical than the other. I'm having basically the same problem as you right now in that my sensor readings are too noisy, meaning I can't reliably use the accelerometer as an orientation reference. No amount of filtering is going to turn random noise into meaningful data.
If you have your sensors "hard mounted" on your frame, unless your props are *perfectly* balanced (and even then, really) you're going to have noise issues. You need to mechanically isolate the board from the quad and dampen as much vibration as possible *before* it gets to the sensors. I'm thinking of mounting my board to an 1/8th inch aluminum plate (for mass) and then using moongel to stick the plate to the quad, along with some rubber bands or something to make sure it stays put. Also thinking of just using some small pieces of Zeal on the corners of the plate. Using very small pieces of whatever material you're using means you don't need as much mass sitting on it for the damping to be effective.
Watch this for some ideas: http://www.youtube.com/watch?v=h3y4Clyf1cw
Other reading: (I'm going to be trying a bunch of these myself, when I get some time to do so)
https://code.google.com/p/arducopter/wiki/VibrationControl
http://diydrones.com/forum/topics/vibration-isolation-and-dampening-of-apm-px4-for-version-2-9
http://diydrones.com/forum/topics/a-new-approach-to-vibration-damping-at-the-source
http://diydrones.com/profiles/blogs/vibration-control-wiki-section-started-for-better-or-worse?id=705844%3ABlogPost%3A1130051
http://fpvlab.com/forums/showthread.php?4251-Vibration-Dampening-amp-Isolation-Solutions-Guide
There are lots of ways to do it, but *anything* is better than nothing. You want to get the sensor vibration as low as you possibly can. It'll mean you have to do less filtering, and your results should be significantly more accurate.
Those links were good also, I already had the top one bookmarked. I was(am) going to try the double sided tape on my new frame. I think I am also going to rewrite my IMU code 1 more time before I switch frames and motors. My bench top tests with the median filter look promising. I want to see how they hold up in a real flight with motors vibrating.
Have you played with a median filter on your quad? My version of it is kind of slow, it takes about 1.5 mS to run my 3 values through an 11 point filter.
My thought process right now is that I am going to code this thing and hopefully have my flight loop running at 200Hz still. My code will need refined and I will have to learn how to code better, but in the back of my mind I am thinking that if I can keep it at 200Hz or maybe even 100Hz when the Prop 2 comes out that will double my speed. I know the prop 2 isn't coming out for awhile but it will take me that long probably to learn everything. Also if Bean and Batang come out with the new Prop Basic Studio that might be a way to speed it up. The best thing for me to do is to get better with coding and learn more Trig. I think I read that the new prop will have native multiplication and divide, that will be sweet if i read that right.
Shawn
Your median filter would be better done in PASM on the cog that's doing the sensor reads. It wouldn't be trivial code to write, but it'd go like a scalded cat.
Jason, did you go from gyro only to DCM or did you try a kalman filter or complementary filter in between.
Just curious is all.
I have the most problems with returning my quad to level after I have done something like this. Pitch forward 35 degrees and apply yaw heavily at the same time, if I let off the pitch and yaw completely all hell breaks loose. I maintain attitude pretty good with mulipliers of .999 and .001 for my comp filter no matter what I do as long as I do not yaw heavily, actually I can yaw heavilly as long as my quad is reasonably level. So then when I apply yaw I will scale those two values from lets say .999 and .001 to .996 and .004 depending on how much yaw I have applied.
I don't think I have as much of a problem with noise as I thought coming from my ACC. I actually think it is more of a problem not understanding the comp filter, which I think I understand now. I have been sitting here rotating my board in all kinds of directions making note of how quickly it responds to changes in orientation. If I put too much ACC in the equation I think it is killing part of my Gyro reading, which causes a lag in response. And if I put too much gyro it adds a huge lag in its response to yaw forces on the board.
None of that probably made sense, its a good thing I am not a teacher.
Shawn
Am I off my rocker? Or has anyone tried to do this before? I have not found an example of anything like this. No one probably has a clue what I am talking about because my brain just exploded all over the screen in an unorganized manner.
Shawn
I am going to finish my third version of the flight controller, however I am not anticipating great results. Altering the alpha value of my comp filter seems to be working while I move the board by hand but I am not sure how it will work in a real application.
From what I have been reading no matter what fusion algorithm is used they all have problems with centripetal force. I would assume this has to do purely with the sensors capabilities. After research and experimenting I am pretty sure my main problem is the fact the gyro gets lost when an axis is tilted and yaw is applied. With a varying alpha I have been able to reduce the lag in this situation. I think my gyro is solid, I am using a alpha of .9995 during attitude hold and a down to .9980 during extreme flight. Like I said I have not actually tried the new flight program yet, but I did use the .998 hard coded as alpha in my last version and it flew but had a hard time at maintaining attitude.
DCM here I come.
I think that a comp filter will work fine for just maintaining attitude, but to change from flight to maintaining attitude it might be lacking. Hopefully I will have my third version running by the forth. Initially I am going to leave the median filter out.
Shawn
This is a quote from a link I posted in post #50.
I am trying to decide which way to go next. Try to implement the formula from this article to deal with the yaw problem or switch over to DCM.
http://scolton.blogspot.com/2012/09/fun-with-complementary-filter-multiwii.html
If DCM still uses a comp filter to fuse the acc and gyro data I would imagine that the yaw problem still exists since the acc is fed into the final angle slowly.
I am curious how others are dealing with this problem. If the DCM proccess has the same limitations I think that this link may be helpful to those of you using DCM also.
I attempted to figure out the equations from that link before, but could not grasp it, my math understanding is a little better now so I think I may try again.
Thanks
Shawn
Try this - Start with your quad in front of you, pointing forward. Turn your quad upside down by rolling it 180 degrees. Now change the heading by rotating left 90 degrees. Now roll it back to upright again. Now, where does your quad think it's pointing? Because it was upside down, rotating it to your left is actually rotating to your quad's right. The sensors you get your readings from are attached to the quad, so they rotate with it. So when your quad is upside down, your sensors are upside down too, and the readings aren't aligned with the world any more. Your quad thinks it's pointing to your right. Where it believes itself to be is 180 degrees away from where you believe it should be.
This is an extreme example, but it's exactly the problem that DCM and quaternion based IMU systems deal with. You take your current orientation (quaternion or matrix) and update it using readings from your gyro and accelerometer. Since those readings happen frequently, the changes to the orientation are relatively small. You then figure out where the ground "should be" based on this orientation value, and you compare that to the acc reading. You figure out the difference, and apply some amount of that as an additional rotation to re-align the orientation with the acc. (this is the complimentary filter part). All IMU's that use an accelerometer are going to have issues with centrifugal force, but that's not the same problem as tracking the rotating orientation frame.
Start reading up on rotation matrices - If you can wrap your head around the basics of those, the DCM code isn't that hard. Pay particular attention to this bit:
One can interpret the row vectors of a rotation matrix as unit vectors along the axes of the rotated space, in the original coordinate system. Likewise, the column vectors represent unit vectors along the axes of the original space, in the rotated coordinate system.
Jason, that pretty much determines where I will go from here. DCM here I come!
I am glad that I have worked my way through this comp filter stuff with your help. I am finally getting a pretty good idea of how things work and what I need to do, to make my quad fly. Don't get me wrong, I am no where close to having a quad that flies well, but pushing my way through this has taught me a lot about all sorts of things.
Right now I just want to fly something, I am kind of at that point where I want to fly instead of build. I have been playing with the equations from the link that I posted earlier today. I am actually getting the math to work now, but it is all in floating point. It seems kind of slick how it works, my last attempt with the comp filter will be to try and apply the yaw compensation from that post. Hopefully I can get something that is enjoyable to fly and not just hover. No matter what the outcome, I am going to jump into DCM next.
Thanks Jason
Shawn
Shawn
http://scolton.blogspot.com/2012/09/...-multiwii.html This is the link I found the code at. I know I posted the link just a few posts ago, but I do not like to take credit for other peoples work.
I was amazed at how well it hovered, I would have to say it out performed the plain complimentary filter. Tomorrow I am going to take it to the park if it is not windy, and test out how well it works for yaw compensation. On the bench it works perfectly. But I also said that about the code I wrote last night. Mr Coltons code is way more advanced than mine. Here is a link for the yaw compensated comp filter. I wrote a short demo to see how it responded. The IMU I used was a sparkfun 6DOF board.
Comp_Filter_W_Yaw_Compensation_Float.zip
Depending on how well my flight goes tomorrow this maybe the end of my journey with the comp filter, at least for this project. If all goes well I will post a video, my next task is to dive into DCM.
Thanks
Shawn
Now I have something to fly while I am learning DCM. I will post a video as soon as I have time.
Just for the hell of it I am going to slow my flight loop down to 50Hz just to see how it handles. I will post a video of that also.