Shop OBEX P1 Docs P2 Docs Learn Events
Filtering Sensor Readings (Kalman Filter) - Page 4 — Parallax Forums

Filtering Sensor Readings (Kalman Filter)

124

Comments

  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-03-26 12:53
    The PID loop *looks* fine - there's nothing magic about it, and the code is pretty simple, so I don't think that's your issue.

    If I had to guess, I'd say precision, but that's only a guess. Without actually testing the code and seeing how it responds, I can't say for sure. I know that vibration can cause problems with these sensors, so if you have a lot of it that may still be an issue.

    Try adding some D to your PID, but pass in the current GX to use as the DError, instead of computing it. GX is the instantaneous gyro reading, so it's basically the same as the difference in rotation values from one loop to the next, but it'll be more accurate. Mine uses that, and it made quite a difference.
  • ShawnaShawna Posts: 508
    edited 2013-03-26 18:19
    Jason, I understand what you are saying about using the GX value as the DError, but won't that fight the P portion of my PID loop since I am trying to control the quad wirh a desired angle. If the control board is trying to maintain a level hover I can see how that would be benefictial. But if I want to maintain a pitch of 10 degrees and I feed that into my PID loop as the set point, won't the D portion of the PID loop fight the P portion?

    Thanks
    Shawn
  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-03-26 19:03
    It will, but the P portion can be made high enough to overpower it.

    I think you may simply need to use the D component anyway - Your P error value isn't going to vary enough to make the quad respond to small changes, but the D error will.

    In your case you have two things you're trying to accomplish:

    1) Stabilized flight
    2) Level hover

    The D factor is going to get you #1, and part of #2.
    The P factor is going to get you #2, but not #1.

    If you're level, and start drifting away from level, your PError value will change very slowly, but the DError value will be much higher. Incorporating that into the code means you'll be reacting much faster, which is what you want.

    You may also need to negate the sign of it - Just be aware of that when you start testing - it may "run away on you", so start with small D values, and use the serial terminal output to make sure that it's responding well before turning it up. You may even try turning off the P factor completely. You should be able to get "stable" but not level flight using just the D factor. Most people use P, but they're using P on the gyro value, not their angle value, so their P is the same as your D. Does that make sense?
  • ShawnaShawna Posts: 508
    edited 2013-03-26 19:23
    That makes sense, I appreciate your help. You have given me a lot of new info to try. I still need to build a stand, it shouldn't take to long once I have time to go and work in the garage.
    One more question just out of curiosity, in your DCM code are you feeding angles into your PID loops? I don't understand the DCM code fully but it looks like one would have to convert to angles in order to maintain attitude. Or I guess maybe the when the matrices are transposed together that converts the values to radians, I don't really understand the DCM enough to ask an educated question on them.
    Thanks Again
    Shawn
  • ShawnaShawna Posts: 508
    edited 2013-03-27 19:48
    Hey guys,
    I finally had some encouraging results. Jason I did not know what negate meant, but after feeding my Gyro rates into the DError variable for my PID and holding the quad in my hand I figured it out. I don't understand why that is necessary, I don't have to do that in my angle calculations for the P portion of my PID. Anyways I set the P for all 3 axis to 0 and tried lifting the quad off the ground. The quad drifted and spun but was very stable, no oscillation or flipping. So I added some P to my 3 axis's and tried to hover again. Very stable but very poor control, I am very happy, I was about to throw the stuff in the closet and take a break from it. I didn't have the quad off the ground very long, but I could tell it was stabalizing pretty well. I probably only had it off the ground for maybe 1 or 2 seconds max. Now I can start obsessing again! (:

    I am sure I will have more questions.

    Thanks Jason

    Shawn
  • ShawnaShawna Posts: 508
    edited 2013-03-28 15:59
    Its flying, I added a little more P and it was off to the races. There is still a lot of tweaking that needs done but I am very happy with the results so far. I will try to take a video and post it, but I am not real sure how to do that. Anyways, thanks guys. I will have more questions but I am supper excited.

    Shawn
  • ShawnaShawna Posts: 508
    edited 2013-03-28 18:03
    Here are some pictures of my Quad, there is a picture of my 6DOF board mounted to my proto board before and after I re-supported it. Also there is a bad picture of the acrylic mount I made for my control board.



    DSCN2150.jpg
    DSCN2151.jpg
    DSCN2152.jpg
    DSCN2153.jpg
    DSCN2154.jpg
    DSCN2155.jpg
    DSCN2156.jpg
    DSCN2157.jpg
    DSCN2158.jpg
    1000 x 750 - 121K
    1000 x 750 - 100K
    1000 x 750 - 165K
    1000 x 750 - 163K
    1000 x 750 - 149K
    1000 x 750 - 179K
    1000 x 750 - 97K
    1000 x 750 - 118K
    1000 x 750 - 198K
  • ShawnaShawna Posts: 508
    edited 2013-03-28 18:49
    Here is a terrible video of my first flight outside. It was a little windy and I need to balance the quad and trim it also. I had to constantly apply stick input to keep it from drifting away. At about the 2:00 mark, I kind of got a feel for its drift and had some decent hover. I like how the motors sound, they run pretty smooth.

  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-03-28 23:39
    Awesome! Congratulations!
  • A. ESERA. ESER Posts: 24
    edited 2013-03-30 14:13
    +1
    Congratulations!
  • ShawnaShawna Posts: 508
    edited 2013-03-30 15:44
    Thanks for the encouragement guys.

    Ok, next question. I have got the quad to hover pretty stable by trimming the roll and pitch axis and also by trying to zero the accelerometer. I get very little to no oscillation during hover and if I apply pitch or roll input the quad is pretty responsive. When I let go of the pitch and roll stick the quad pops back and tries to maintain a hover and does a pretty nice job of it. However it does overshoot a little bit when it pops back to a hover. I am not sure if I have a little to much P or D in my PID. In my code the raw gyro rate is fed into my D variable and the angle of the quad is subtracted from my desired angle in my P variable. I have tried adjusting the values with little success. Have you guys seen this before?

    Also I can't quite get a perfect hover it has a little drift. The D term stabilizes the quad and the P term tries to level the quad, do you guys think I could fix this by adding a little bit of the I term into my PID loop? Its like the P (proportional) can't quite get rid of the drift, but if I add I term to the PID I am thinking that the little bit of error will add up and fix the problem.

    And finally my biggest problem that I can see right now. If I fly the quad in calm conditions it handles pretty good, but if there is any wind and I try to hover it freaks out and starts to oscillate really bad, almost to the point where I can't control it. It has to be in the PID's but I am not sure which part. I have heard of some very expensive commercial flight controllers having this problem. What do you guys think about this problem?
    Thanks Shawn
  • ShawnaShawna Posts: 508
    edited 2013-03-30 18:43
    Well the wind went down luckily and I had time to take the quad outside. There was still a slight breeze and the quad was oscillating a little. So I increased the P and D values and tried again, it was pretty smooth. I still had some drift but I think it was mainly the breeze. I suppose that makes sense to have to increase the PID values, the wind would be an external force. No wind inside, so no problem. I wonder if a person should have the PID values turned up as high as possible without causing oscillation? I think if the PID values are too low there is lag in stick input.
    I am super happy with this so far, I haven't really been able to push it, I have been for the most part just hovering. I still need to work on my Yaw axis, it still drifts pretty bad. That should be fairly easy to fix, or at least improve. I swear this thing flies just as good as my multiWII board, or at least it hovers as well.

    I have a lot of things in the code to clean up and improve, but my brain is already thinking about heading hold. What brand of magnetometer are you guys using? I have done a little reading on 3 axis magnetometers and it looks like writing code to calibrate one is tricky.
  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-03-30 21:44
    I don't have a magnetometer - I use the exact same PID code to hold heading as the other two axis (different settings, but same code), and it works well.

    You mentioned that the quad is drifting (IE that you can't get a perfect hover). Do you mean that it's drifting laterally, or that it's tilting? If it's tilting then you probably need to tweak your "level" value, or start adding I into the PID. The I term is the one that can fix that last little bit of error. If you mean lateral drift, IE that the quad won't stay in one place, you need GPS for that. Level and stationary aren't the same thing at all. :)

    Congratulations again on getting it flying. Your enthusiasm is infectious - I took mine out for the first time in a while, and managed to "tighten up" the control quite a bit - I was flying figure 8's around the little park by my house which I haven't been brave enough to do before - it always felt a little mushy. I applied the advice I gave to you and boosted my "D" term a bunch and now it's solid. So, thanks!

    Jason
  • ShawnaShawna Posts: 508
    edited 2013-03-31 05:54
    Cool, I am glad you got yours out and tweaked Jason. I remember reading somewhere that you had crashed yours and when you rebuilt it it never flew quite the same. It sounds like it flying now, awesome!

    I realize that a person needs GPS to keep the quad stationary in one place, however if everything is balanced and tuned and there is no external forces being applied to it, the quad should not move much. I meant that it is tilted, not real bad, but enough that it drifts pretty fast. From playing with the multiWii code I have found that there is a difference between trimming the quad and trimming the ACC. I have been led to believe that you cannot trim the ACC accurately unless the rest of the quad is trimmed. Which I think this means turning off the ACC and lifting off the ground with just throttle and seeing where it veers off to. And then trimming the motors, once it is close to trimmed this way then one can trim the ACC.

    I am going to add some I to my PID loop and see what happens, I need to slow my head down and remain task oriented because there are so many little things that need to be tweaked. One problem at a time.

    I have been grounded all winter because of the weather and so my skills are a little rusty. I started flying quads last summer and was getting the hang of it but I am a little rusty now. I need more practice, I need to react when flying instead of thinking. Thinking takes to much time. If that makes sense.

    Anyways got to get ready for church, Happy Easter Everyone!

    Shawn
  • ShawnaShawna Posts: 508
    edited 2013-03-31 12:51
    20 MPH winds with 30 MPH gusts today. Took the quad out to see how it flies in wind since my last PID tweaks. No oscillation, which is good and obviously was not fun to fly, but the test was successful.

    Shawn
  • ShawnaShawna Posts: 508
    edited 2013-05-12 18:42
    Hey guys,
    I am still using the Complimentary filter for my quad build. I am going to try and add a complimentary filter for my yaw control next, using a compass and the gyro. I have also added an LCD to the quad so that I can configure it in the field without a laptop. Here is my latest video, I was mainly trying to see if the quad would go back to level flight after stick input.

    Shawn
  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-05-13 12:39
    Looking good! At around that 3:00 mark, was that something you did, or something it did on its own? Just curious. I've had a few of those. :)
  • ShawnaShawna Posts: 508
    edited 2013-05-13 20:03
    The flight didn't go as well as I thought it would, but it was decent. I was having problems with yaw control in the video. I had a couple flights in the day with no wind and it flew really nice. One of the arms on my quad is twisted slightly and all of them are bent from crashes. All but one of those crashes occurred with a multi wii board. I'm not bashing the multi wii board either, it is a great piece of software. Also I have my throttle limited to 1650uS to leave room for overhead for my PID loops. I need to increase that, there isn't quite enough thrust there. My biggest problem is lag in my ACC value. After flying aggressively in any direction when I return the quad to auto-level it does not level out right away. Its close but it can take 10 to 20 seconds for it to level out completely. I am pretty sure that the reason is to much filtering of my ACC, but I have not tried to adjust it. Last weekend was the first time in a long time that it was not windy and I was just having fun flying something I built. Its windy on the plains.............all the dam time! I need to build a wind generator but then my property taxes would go up. My goal last weekend was to finish my lcd and get a little flying in. Mission Accomplished.
    I also played with a magnetometer a little also.
  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-05-13 21:21
    I had problems with the ACC pulling my level value off too, and I fixed it by reducing the amount of ACC in the mix. If you've got it set to 98% gyro and 2% acc, it might still be too high. The acc value should affect you over something like 5 seconds, not one second. At 250 updates per second, 2% acc in the mix is actually a decent amount of contribution. If your gyro readings are accurate, you don't need much accelerometer in there.
  • ShawnaShawna Posts: 508
    edited 2013-05-19 16:06
    I went back and looked at my percentages for my Comp filter. I did find a problem with them. I thought they were set for 99% and 1% but in reality they were set for 99.9% and 0.1%. With Comp filter settings like this I think I would have major problems with what Jason was talking about in post #22, non-commutative rotations, at least that is what I think was going on. When I applied 30 degrees of Pitch input, which causes the quad to fly forward pretty quickly and then I added yaw at the same time it still flew, but when I returned the pitch and yaw stick to neutral all hell would break loose. The quad would not maintain attitude it would veer off in a direction. From here if I would try to level it out manually with stick input after 10 to 30 seconds everything would be ok. Does that sound like not enough ACC mixing?

    Now, the reason I have not been able to retest. I thought I had the Comp filter set to a 99:1 ratio. So I went to the extreme of 50:50 ratio, not a good idea. I started hovering it and everything seemed OK. I applied pretty hard roll for just a fraction of a second and then let off, the quad did not go back to level flight, it ran into my fence.:frown:

    I do not understand this, was this probably because of noise in the ACC? I had also turned off my ACC pre filter before I did my first Comp filter adjustment.

    Anyways, I got frustrated and started playing with my 9DOF board, basically going over my old program and writing a new one when I found this error.

    And finally one more thing I would like to run by you guys. The ACC gives me 3 values, with these values I get pitch and roll angles to dump into my Comp filter. Well the ACC is noisy and filtering these values takes time, so I thought to myself on my first program. Why not filter my computed angles from my ACC instead of the raw values. Then I can filter 2 numbers instead of 3(Pitch and Roll VS aX,aY, and aZ) which should save time. Well on my latest version I decided to filter the 3 raw values coming from the ACC and then compute the angles pitch and roll from the filtered values. I have only bench tested this but I believe the smaller numbers are easier to filter and save on lag time developed by the filter itself. Maybe I am wrong, or maybe I am clueless and that should be a no brain er. Also the accuracy of the atan2 function should be better. Throwing two noisy signals into a calculation and then filtering the calculation sounds like kind of dumb idea, now that I have thought about it for the 100th time. So maybe that puts this into the no brainer category.

    My skull maybe thick, but I swear there is productive activity going on up there.:smile:

    Shawn
  • ShawnaShawna Posts: 508
    edited 2013-05-24 10:29
    Hey Guys,
    I am putting my new flight controller together and for the life of me, I cannot find the VCC IN max rating for this 9DOF board from sparkfun (https://www.sparkfun.com/products/10724). Does anyone know it off the top of their head. I cannot read the number off of the regulator and I don't see the spec on the sparkfun site. I am running 3.3v to it now, but I want to run 5v to it.

    Thanks
    Shawn
  • LeonLeon Posts: 7,620
    edited 2013-05-24 10:48
    Why don't you ask Sparkfun?
  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-05-24 12:53
    A quick search of the comments reveals this: "The voltage regulator is the SM MIC5205 Series 3.3 V 150 mA."

    http://www.micrel.com/_PDF/mic5205.pdf

    So, in theory you could feed it up to 16V. I think 5 should be ok too. :)
  • ShawnaShawna Posts: 508
    edited 2013-05-24 13:09
    Thanks Jason I most of missed that while I was skimming through the comments.
  • ShawnaShawna Posts: 508
    edited 2013-05-25 07:15
    I have got my quad put back together, I haven't got my new board completed yet but I am about to test my new program. I took the quad out with my original board and my first program and did some more test flying.

    This is the conclusion I have come too. The little success that I was having I believe was all due to my gyro. My major error in thought was that my integer filter numbers for my complimentary filter where 99% and 1%. When in reality I was mixing 99.9% and .01%. When I switch my gyro to 100% and my acc to 0% there is almost no change in how my quad flies. The quad does get out of whack on its attitude hold faster and never is able to get it back without that .01% off ACC.

    If I change the complimentary filter percentages to 99% gyro and 1% acc the quad is very unstable. My conclusion is that there is to much noise in my ACC. If I increase the filter on just my ACC then there is so much lag in stick commands that the quad is almost un-flyable. I am hoping my new version of the flight controller will fix this problem. In the new version I am filtering the raw ACC signal before I dump the numbers into my ATAN2 function to calculate the angle. In my current flight controller program I am getting the angles out of a cordic table and then filtering the results.

    I am not sure what math is required to perform the ATAN2 function, but I suppose if there is multiplication involved, not filtering the raw values from the ACC before calculating the angle could actually be magnifying the noise.

    At least that is what I am hoping the major problem is, I have done all I can do, to try and isolate vibrations on my frame. I did nothing to this frame when I had another flight controller on it, and it functioned fairly well.

    Hopefully I will get my new program running this weekend.
  • ShawnaShawna Posts: 508
    edited 2013-05-28 16:47
    Well, I still am having problems with a noisy ACC. I have 1 more filtering attempt with my low-pass setup and then I think I am going to try something new if it fails. If I apply a heavy low-pass filter to my signal it does get rid of the noise I believe, but the lag in the ACC output is terrible. Actually I think what I am calling lag is actually a phase shift in the signaling due to the heavy filtering.

    So my question is should I try to apply a 3 point median filter, or is this a waste of valuable time in my flight loop? It looks like a median filter will get rid of noise and not have a phase shift. Also it looks like a light low-pass filter can also be used in conjunction with median filter.

    I do not fully understand the median filter but I think I have a pretty good grip on a simple one.
    What do you guys think?

    Thanks
  • ErNaErNa Posts: 1,752
    edited 2013-06-03 00:46
    I received another newsletter I'd like to share here, its a focus on Kalman filter, maybe it's useful. But: no guarantee ;-) http://e.ubmelectronics.com/audience/UBMTechNewsletters/05-28-2013-EMB-Tech-Focus.html?cid=Newsletter+-+Embedded.com+Tech+Focus
  • ShawnaShawna Posts: 508
    edited 2013-06-19 16:11
    ErNa, Thanks for the Link, there is a lot of info there.

    I have had a little time to play with my quad some more. I am still having problems with noise in my ACC readings. I have tried a few different things with low pass filters with no success. I have been trying to put together a median filter for the last couple of weeks. I finally have something running but not in my quad program. I read on a website that a guy had good success using a median filter and a very light low pass filter but I cannot find the link again. So my question is....... should I apply the low pass filter before or after my median filter? I was thinking before but I am not really sure.

    I order a DJI Flame 450 kit hopefully this frame will be less susceptible to noise.

    Thanks
    Shawn
  • JasonDorieJasonDorie Posts: 1,930
    edited 2013-06-19 22:17
    After - The purpose of the median filter is to remove spikes in the data (ie - outliers). Doing the low-pass before them will have a similar effect, but with different weighting. It'll be more effective doing the low-pass after the median.

    Try it yourself - make up some "smooth" data, add a little random noise to it, then add a few spikes. Graph the result of doing them one way, then the other, and compare them.
  • ShawnaShawna Posts: 508
    edited 2013-06-21 18:03
    Using the median filter makes sense.

    Edit
    I meant to say applying the median filter before the low pass filter makes sense.
    Edit End

    If anyone is interested I found a basic article on complimentary filters and fusion of gyroscope and accelerometer data. It gives the complimentary formula. Also what interested me the most is that it gives an equation for calculating the complimentary filters gyro and acc mixing percentages based on the control loop time and another value that I have not figured out yet. Here is the link.
    http://web.mit.edu/scolton/www/filter.pdf
    S
    hawn
Sign In or Register to comment.