Filtering Sensor Readings (Kalman Filter)

Shawna · 2013-01-04 10:23

Hey Guys,
I am trying to put together a Quadcopter. Right now I am trying to get just the gyro working to help stabilize the quad. I am using Jason Dorie's HoverSport program as a blue print for what I am trying. If I missed a co author on that program I apologize. I realize that you do not need a Kalman Filter for just the gyro but eventually I am going to add a accelorometer.

I have a flight controller now that I use Multiwii software on. In the multiwii software you can set filters on the sensors, on the gyro it can be 98hz or 148hz or so on. My question is, what is that 98hz, how do they implement that?

Most of the stuff I do is with temperatures and stuff related. To smooth out my readings I will take 10 to 20 readings and just average them.

Is the 98hz on the multiwii just how often the reading from the gyro is read? If so how is that actually filtering anything. I would think you would want to read the sensor as often as possible. Or when they say 98hz are they taking 98 readings and then averaging them once a second? That doesn't sound like a good idea either, that would put a lot of lag in the reaction of the quad.

I just am not getting the whole picture, I got parts of the picture and have tried to put it together but I think I still missing something. I just can't put it all together.

Edit 03-11-13 Because of the disscusion in this thread I have decide not to use a Kalman filter, I am instead using a Complimentary filter. So this thread deals mainly with Complimentary filters and me just rambling on.

Thanks
Shawn

SRLM · 2013-01-04 11:24

Generally, the XXHz specification of an inertial sensor specifies how often the sensor chip takes a reading (or sometimes it's "output data rate"). This XXHz number is implemented on the MEMS chip itself. There is no filtering here.

There are three reasons that I can think of to not run the sensor as fast as possible:
1) Some sensors decrease their resolution at higher speeds. So, instead of 14 bit resolution at 60Hz you get 8 bit resolution at 120Hz (for example).
2) Your processor can't process data that fast. So, to get a more consistent delta-t between readings you slow it down.
3) Higher data rates take more power (this is a pretty weak reason, though, since modern MEMS devices are pretty efficient no matter what speed you run them at).

I'd suggest reading through the datasheets of some gyros and accelerometers to get a better idea of the technology.

JasonDorie · 2013-01-04 14:49

If they're calling them "filters", as opposed to just "sampling frequency", chances are it actually is a real filter, which means that it won't allow signals through containing waveform frequencies higher than 98hz, or 148hz, etc. That means they're probably sampling significantly faster than that (at least double) and using moving average, windowed FIR filter, or something along those lines, to smooth out the data.

As for using a Kalman filter, they work well in 1D, but get complicated if you're trying to use them for 3D motion.

The problem has to do with the non-commutative nature of rotations. If you rotate a craft with a gyro on it 90 degrees to the right, it's orientation is now different than that of the world around it. If you then pitch the nose of the craft 90 degrees foward (relative to the craft), the nose didn't move "down" in the world, it rotated counter-clockwise around a vertical line. Now if you tilt 90 degrees to the left (again, relative to the craft), you're upright again, and facing a different direction than when you started, but you got there without ever just "turning to the left".

The DCM stuff posted here deals with this problem by constantly updating a matrix which is the current estimate of the orientation of the craft in the world:
http://forums.parallax.com/showthread.php/131022-Propeller-DCM-Now-with-source

It's dense, but it works, and it's easier to understand (and less magical) than a true 3D Kalman filter.

Shawna · 2013-01-16 18:49

Ok the kalmans filter is to advanced for me to start off with, I can barely read my gyro and acc sensors.

This is the first thing I want to do. I want to take the raw value from my acc and convert it to an angle. I am using an equation I got off a tutorial on the internet.

Angle = atan2(accx,accxzero) +PI

I am using an ADXL345 and the output scale is 4mg/LSB. The ADXL345 is on the sparkfun 6dof board.
Now I am assuming that accx is my raw reading divided by .004g.
But I do not understand the accxzero, the value is 0. If I am understanding the float32 Lib right atan2 is atan(accx/accxzero). Anything divided by zero is zero, I don't get it.
If I set my proto board on its side 90 degrees from being flat I get a raw reading from my acc of 263 on my x axis.
263/.004 = 65750
ATan(65750) = 89.9999
89.9999+PI = 93.140
I think that result is close since I did not use a square to align my board. Like I said I don't understand the accxzero. Or is accxzerro supposed to be my 4mg LSB?
This is the section from the web I was looking at, it was actually in an arduino forum.

The accelerometer
The accelerometer measures the acceleration (g's) in three dimensions. To translate the analog readings into degrees you simply need to read the axis and to subtract the zero offset like so:
accVal = accAdc-accZero

Where accAdc is the analog reading and accZero is the value when it reads 0g - this is calculated in the start of the code, look in the "Setup" section. The zero value can also be found in the datasheet: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. You will see that the zero voltage at 0g is approximately 1.5V, to translate this into quids, you again have to use this equation: zeroVoltage/3.3*1023.
So in this example I get:
1.5/3.3*1023=465.

You can then calculate the pitch and roll using the following equations:
pitch = atan2(accYval, accZval)+PI
roll = atan2(accXval, accZval)+PI

Atan2 has a output range from -π to π (see http://en.wikipedia.org/wiki/Atan2), I simply add π, so the range it converted to 0 to 2π.
To convert it from radians to degrees we simply multiply the result by 57.295779513082320876798154814105 - this is predefined in the Arduino IDE as RAD_TO_DEG.

I am lost.
Shawn

JasonDorie · 2013-01-17 13:07

Atan2 is a special variant of Atan that is "quadrant aware" - It takes a 2D vector as an X,Y coordinate and tells you the angle formed between a flat line along X and your vector. Atan and Atan2 both generally return results in radians, though from the looks of your results, your version might be returning degrees. Regular atan has an upper limit of (PI/2) or 90 degrees. Take a look at the diagram about 1/2 way down this page: http://en.wikipedia.org/wiki/Atan2

Atan2 of (1,0) = 0 = 0 degrees, a perfectly horizontal line, extending toward positive X.
Atan2 of (0, 1) = Pi/2 = 90 degrees, a vertical line, extending upward.
Atan2 of (0, -1) = -Pi/2 = -90 degrees, a vertical line extending downward
Atan2 of (-1, 0) = Pi = 180 degrees, a horizontal line extending toward negative X.

If you want to determine your roll angle (IE, tilt left to right), use Atan2( AccZ, AccX ). If your craft is at rest, AccZ should be a positive number representing 1g, and AccX should be zero. If your craft is tilted to the right, AccX will be a positive number, and AccZ will reduce. At 45 degrees, AccZ and AccX should be approximately equal to 0.707 times the value at 1g.

To compute your pitch (IE tilt front to back), use Atan2( AccZ, AccY ).

The AccZero value they're talking about is generally if you're reading an analog accelerometer using an analog to digital chip. In that case, the ADC can't read negative values, so they set the readings to be centered around 1/2 of your voltage range. A value of -1g would be analog 0v, and a value of +1g would be analog 3.3v. Since the ADXL is a digital chip, I think it handles all that for you, meaning you should get back signed numbers. You can check the range easily by just outputting the raw readings. Hold the board at various angles and see what numbers come out. If all you get are positive numbers, you need to figure out what "zero" is. If you get positive and negative numbers, zero is handled for you. Does that make sense?

The 4mg/LSB means that each "bit" of your reading represents 4mg (milli-g's, not milli-grams). So a reading of 250 should be 1g. (250 x 4mg = 1000mg = 1g)

Shawna · 2013-01-23 08:26

OK I am trying to wrap my head around this, is there any easy way to display Floating Point numbers in P.S.T for debugging purposes? I thought I saw an object in the obex but I can't find it now.
Thanks
Shawn

Shawna · 2013-01-23 13:08

Ok, I figured out how to display the floating point in VB. I wrote a spin program just to simply display the x and y angle of tilt from the ADXL345 Acc. It seems to work but I was surprised to see that the resting value is 180 degrees for both x and y axis, not surprising they are the same but I thought the resting angles should be 0. Oh well not a big deal.
Here is the code if anyone is interested, I used the 6DOF from sparkfun, and the ITG3200 object to read the chip.
Next thing to figure out is determining the angle from the gyro and then from there probably a complimentary filter and then to the Kalman Filter. Eventually I want to get to the DCM but I am going to take it in baby steps.
Thanks for the help Jason and SRLM, also Unsoundcode helped me a great deal with the VB side of my project.
Shawn

ACC Angle Reading FP32.zip

Shawna · 2013-01-23 15:45

Ok, I am not quite done with the qyro angle project, but I have a working axis and now understand the the qyro drift I think. I started reading on complimentary filters and I think that now I kind of understand the equation I have been looking at.
angle = 0.98 *(angle+gyro*dt) + 0.02*acc
I am guessing that the gyro and acc variables in this equation are calculated angles. If I am starting to understand this filter it looks to me that only 2 percent of the ACC angle is mixed in with 98 percent of the gyro reading equaling 100 percent combined of both angles. Is that correct or am I still missing something. As much as the gyro seems to drift this doesn't seem to be very much of the acc being mixed in.
Does centrifugal force affect the reading of the gyro like it does the acc?

Shawn

JasonDorie · 2013-01-23 16:26

That calculation you're referring to is essentially a feedback loop. The new angle value is based on the old one, with two new readings added in.

Angle = (Angle + Gyro * dt):

This is the current angle value, plus the gyro reading multiplied by the difference in time since your last reading. If you take 100 readings per second, and you're rotating at 20 degrees per second, your gyro * dt value will be 20deg * 0.01, or 0.2 degrees. Do that same calculation 100 times and your new angle value will be whatever it was plus the 20 degrees per second you were rotating for that one second. This requires that your angle value and your gyro value be in the same form - IE degrees, radians, or whatever other form you decide on, and that dt is a fractional (float) value.

Angle = 0.98 * (Angle + Gyro * dt) + (0.02 * acc)

This one is identical to the upper one, with the exception that you're using 98% of the gyro reading and 2% of the computed angular value from the accelerometer. So, over time, if you're not rotating at all, the gyro readings will be zero (minus a little noise) but the accelerometer values will be an actual angle, so over time the accelerometer will pull the angle value toward it.

Centrifugal force doesn't affect the gyro at all - it just measures rotational velocity. The only reason you need the accelerometer at all is to try to figure out where "down" is. Averaged over a few seconds, a bunch of readings from the accelerometer will be basically right, but at any given instant they're subject to a lot of noise, which is why they're mixed in very slowly. On the other hand, at any given instant, the gyro is quite accurate at telling you how fast you're rotating, but summing those results over time is going to be an inaccurate picture of where you ended up.

Shawna · 2013-01-23 17:57

Jason,
When I said centrifugal force I was referring to the accelerometer, and I thought I had read that in your DCM post. I haven't read back through that post yet. Actually I think what you said was that angular velocity affects the accelerometer, I am not sure.

I noticed with the accelerometer that if you tip the board on the X axis from flat to 90 degrees that the formula above that I posted will read 90 degrees. But, if you then tip the Y axis 90 degrees at the same time, the x axis no longer measure 90 degrees. Is that part of the error from the accelerometer or am I messing up my math? If the board is laying flat on the table and the board is rotated on 1 axis the degrees seem to match what ever the angle of the board s, but if you rotate 2 axis at the same time the angles don't quit match up. I am using the z axis as the second quardinant in my atan2 equation for both x and y axis angle measurements. Is this problem why you switched to DCM on your QuadCopter?

I am still messing with the qyro, however I noticed that if I let the board sit on the table and run, the angle starts to accumulate and no longer is zero. Matter of fact if I let it sit long enough it would climb to an outrageous number. I am assuming that this is the drift that is so commonly referred to on a qyro. Am I right on that one?
Thanks
Shawn

Shawna · 2013-01-24 14:04

Not sure what to do next.
I have a complimentary filter programmed for for one axis using the gyro and acc. Even with that done I maybe a bit ahead of myself.
I think I am having problems with getting a stable gyro reading. I am reading the gyro ever 5mS and then running the numbers through this equation.
Angle = (Angle + Gyro * dt)

I am wondering if my 5mS is not accurate enough because I am getting drift. If I let the board sit on the table and just run through the loop it accumulates error. About 1 degree every minute. Even the complimentary filter cannot fix this.
Is this error normal or do I still have a problem? I am not sure if a person can some how digitally filter this. I kind of thought that the complimentary filter would fix this, but now after playing with it I know I am wrong. I don't no if it is noise on my power supply or if it is a program problem.

I think I need to scale my results to get rid of noise but I am not sure what to try next, I don't fully understand how to scale stuff.

I can post some code but I am not sure anyone will want to look through it.

Thanks
Shawn

Shawna · 2013-01-24 15:47

A couple thoughts I have had but no time to try since last post.
1) I am using Jason's ITG3200 object, my loop in my program is running at 200hz, and the gyro is set for 250hz. I am not sure if this is a problem.
2) A better way to time my 5mS loop. I have some thoughts, but they are not fully formulated yet.
4)I can not seem to get the temp reading off of my gyro, I do not know how much this actually affects the drift, my guess would be that it needs to be addressed or they would not let you read the die temp off the gyro chip.
3)Beer, probably won't work but is fun.
Shawn

Shawna · 2013-01-24 16:27

Another thought, after reading more about complimentary filters. And this thought is based on my assumption that my gyro drifting 1 degree per minute while resting on a table is O.K.
Lets consider just one axis, the roll axis. If my gyro is drifting lets say to + 1 degree while resting and my acc is bouncing in between -.45 degrees and .45 degrees why can't I just reset the gyro accumulation every time the Acc crosses zero.

JasonDorie · 2013-01-24 18:18

I'd suggest making sure that your values are in a similar range - If your gyro is starting at zero, but your measured accelerometer angle is at 180, or 90, or something *other* than zero, they're not in sync, and won't ever line up. Do other basic tests, like make sure if you tilt to the left, that both measurements are affected in roughly the same way - IE, they both become +45 degrees, not one positive and one negative. These kinds of errors are very likely, and will completely ruin anything you did right.

The whole point of mixing in the measured accelerometer angle is to stop it from drifting. It may take a moment to settle down to a stable number if your accelerometer isn't perfectly level, but once the reading stabilizes it should stay there forever.

Regarding your other question, resetting the gyro accumulation when the accelerometer crosses zero won't work because you'll get plenty of false crossings due to noise. The accelerometer is very sensitive to vibration, which is why you're multiplying it by 0.02 - you trust the gyro 98%, but the accelerometer only 2%.

Shawna · 2013-01-24 19:27

I have made sure that both the gyro and accelerometer both start at zero. Also when the board is tilted on the x axis both devices read within a degree of each other no matter whether it is plus or minus and the signs are correct.

A thought just occurred to me that I might not be able to explain without looking at my program, and I am not around my computer.
Should the result or angle from my complimentary filter be fed back to my equation for the gyro to figure out the angle of the gyro output. I think something might have just clicked. Jason you said that the complimentary filter had a feedback, is the feedback fed back to the gyro angle equation, which should then cancel out the drift. That makes sense, at least in my head. I will have to try that as soon as I get a chance. If this is the case, feeding the angle or output from the complimentary filter back into the gyro equation in a sense kind of does reset the gyro drift. I can't wait to try this tomorrow.
Is 1 degree of drift per minute without using some sort of filter about right, or am I mistaking some other problem as drift?
Thanks
Shawn

Shawna · 2013-01-24 19:46

Jason
After reading back through your posts in this thread, I am pretty sure I made a major error in my math equations that basically does away with my complimentary filter. Only if there were more hours in a day. Not only is there a major error I also think I wasted some valuable clock cycles on math that is not necessary. I was pretty bumbed after I spent all day messing with this just to see my drift stick around. But now I see the light at the end of the tunnel for this small piece of the puzzle. I think! I'll be happy for now if it does not drift while sitting on my table.
Thanks
Shawn

JasonDorie · 2013-01-24 22:54

One degree per minute of drift with just the gyro itself is perfectly reasonable, yes. It'll probably be a bit more if you're moving it, but at rest that's a reasonable number.

Shawna · 2013-01-30 18:19

Its been almost a week since I have been able to get back to this. I have the complimentary filter working for the x and y axis I think. When I combine the accelerometer and gyro data my out put tracks the orientation of my board when I rotate a single axis, and then when I lay the board back down the reading pretty much goes to zero degrees. I do not think the table I have my board on is perfectly level.

I am using the x axis of my accelerometer as my roll and the y axis as my pitch. I have a couple questions though.

1) If my table is perfectly flat and I zero my sensors, if I rotate my board on the z axis or the yaw, should my roll or pitch angle move away from zero. They don't move much maybe a degree each, and if I rotate the board back to where it was zeroed, the roll and pitch pretty much go back to zero. Is this normal or is my table maybe not level and the sensors can see that? I did not expect this result, I personally think that it is my table, I can't find my level.

2) If I rotate my board on the x axis to 90 degrees my y axis stays close to zero but as soon as I rotate the x axis to 91 degrees my y axis freaks out and goes to 160 degrees and bounces around really bad. The same thing happens if I swap axis's. My desired angles I think will be 45 degrees at max, and they seem to track together pretty well when both axis are tilted to 45 degrees but I don't know if this results means I still have a problem. This is hard to explain on paper.

3) How many decimal places do you guys think I need for this, my board is sitting on the table and when zeroed I get values of around .1 to .0045 degrees. Is .1 degree enough resolution to maintain attitude on a quadcopter? I am sure that my motors are going to add a lot of noise when I try to run them. The decimal number I get is ridiculous, when I send the floating point to VB it has somewhere between 8 and 10 decimal places. I am not real sure how to scale this either to get rid of some of those decimal places. Any advise?

4) Not really related to complimentary filters but a question just the same, do I turn my signals from my reciever for pitch, roll and yaw into angles and then feed these angles into my PID loops as desired angles. Then the outputs of my complimentary filters will be the actual angles that are fed into my PID loops.

I feel like I am getting close to something, but I am not there yet. I have learned alot though.
Thanks
Shawn

SRLM · 2013-01-30 18:56

@Shawna

1) Ideally, your roll and pitch should remain 0 if it's flat on the table.
2)...
3) Why are you limiting yourself like this? If you're doing fixed point math, then you can probably do something like "16.16". If you're doing floating point, then you're probably using IEEE 754 floating point, which (more or less) adjusts itself to the range you have. What I'm getting at is that you don't really need to choose: it's built into the math. For display purposes, I'd use 2 decimal points (in most cases).
4) I'd use quaternions to represent the orientation. Then, you can put the PID on the difference between orientations. If you put the PID on the roll/pitch/yaw, then I think you'll end up with weird results in the edge cases (it won't always attempt to rotate the minimal amount). Here is my exploration into the process: http://anzhelka.googlecode.com/git/doc/reports/quadrotor_mathematics.pdf

JasonDorie · 2013-01-30 20:19

It's quite possible to do it without quaternions. The DCM approach is actually less mathematically complex, and (personal opinion) easier to comprehend.

Your problem with the axis flipping is probably you using a short vector when putting numbers through atan / atan2. Post your code and I can possibly find the issue with it. Alternately, get your program to display the numbers you're feeding into atan, and also the numbers coming out. You'll probably see a correlation when they flip. It may be worth rotating the values going into the atan function so that it outputs zero when at rest. If you're doing something like adding or subtracting 90 degrees from the output, you'll have to handle the flips - atan2 outputs a number from -180 to +180.

If you're curious about how to do the controls, yeah, you'll probably take the R/C receiver values, scale them to some reasonable angle values in the same units as your measured angles, and feed them in as the set-point values for the PIDs.

Shawna · 2013-01-31 09:26

SRLM thanks for the link, the paper you are putting together is very good. The math in it is over my head but the concepts and blocks in it give a good idea of what is needed to make a quad fly.

Jason, you are right. When my board is sitting flat on a table the acc x and y axis put out 180 degrees. I did not think this was unusual because the board is sitting parallel to the ground. So I do subtract 180 from my acc readings. I thought that I did flip my coordinates around in my atan2 formula, and I thought I was getting a goofy reading of like 267 degrees or something like that when the board was sitting flat on the table. I will have to try it again. It makes sence that the atan2 function outputs 180 to -180 degrees, I over looked that for some reason.

I have read your DCM post a couple of times and probably will read it a couple of more. I looked through the code also and was lost. I struggled pretty hard to figure out the complimentary filter. I think that I understand the complimentary filter now but I am still learning how the acc and gyro react when tilted in different angles.

I don't understand matrices very well, and I looked at the link you sent me in December but I did not understand the math. Do you have a link to a matrices for dummies article? If I dive into matrices I will have to start at the bottom and work my way up.

I am going to continue with my complimentary filter so that I completley understand it, but I do not know if I will ever implement it into my quad. I don't really see a reason why the complimentary filter can not be used to maintain attitude in a quad. However I have not tried a flight yet. After I get the complimentary filter figured out it is off to either PID's or DCM equations, I am not sure which. The kalman filter I think is out of the question. It looks like to me that the math involved with that is ridiculous, and I am not sure I would get the sample rate out of it that I want.
I will rewrite my complimentary filter code and if I can not work the bugs out I will post it and let you guys have a look, but the code I have now is a mess.
Is the DCM code to maintain attitude better than using a complimentary filter or is its main purpose to be able to determine how far the craft has moved without using a gps or mag. I am very interested in the DCM, but like I said I am not that far into it yet.

JasonDorie · 2013-01-31 11:31

The DCM code (and the quaternion stuff that SLRM suggested) both address the problem that rotations aren't commutative.

Multiplication IS commutative. 6 * 2 * 17 produces the same answer as 17 * 6 * 2 - the order doesn't matter. Rotations *aren't* commutative in that the order you apply them *does* matter.

Take a look at this image:

In the left image, start at the red "egg" shape, and follow the green path - that's the "green" rotation. In the right image, start in the same place, but follow the yellow line - That's the "Yellow" rotation. In the left image, the "green" rotation is applied first, then the "yellow" one. In the right image, they're done in the opposite order. You don't end up in the same orientation.

The problem with simple quadrotor code is that it doesn't take this into account. If you rotate the quad by some amount, your gyro tells you how fast you're rotating, and around which axis, but that's relative to the craft itself, at that very instant. It also has this problem: Rotate the craft by 90 degrees in X (relative to the craft), then 90 degrees in Y, then -90 degrees in X. If all you're doing is adding the numbers to running totals, your X result is now zero, and your Y is 90. Hold an object in your hands and do those rotations - it's not the same as just doing a Y rotation of 90 degrees because the rotations aren't commutative.

The DCM and quaternion solutions both generally deal with this problem by representing rotation as complete thing (a matrix or a quaternion), instead of just 3 angles. They also re-orient that complete rotation based on the instantaneous gyro values every frame, constantly updating the estimate of your orientation, and using the accelerometer and magnetometer to correct it as required.

The DCM math took me a couple weeks to wrap my brain around - I read that paper probably 20 times, taking notes, poking at it when I had some free time... It'll sink in eventually. Stick with it.

Shawna · 2013-01-31 17:09

Ok I am still missing something with my ACC calculations.
The raw readings from my ACC while it is sitting on the table are X = 0, Y = 0, Z =250.
The sensitivity of my ACC is .004mg/LSB. The raw readings make sense to me, X and Y are 0 because the board is not moving. The Z value is 250 because 250 * .004 = 1 and while not moving gravity is pulling on the board at 1g.

So to figure the angle of X I do the following.
X = X * .004
Z = Z * .004
Roll = ATAN2(X,Z)
Roll = Roll + PI
Roll = Roll * (180/PI)

My answer is not 0.
I get either 180 or 270 which is not a result of ATAN2.
Those 2 results vary depending on whether I swap the X and Z in the ATAN2 function.
I also tried taking my Z value and subtracting 250 from it before I multiply by .004, but this had unstable results also.
I am not sure what is going on, I am using the Float32 object to do this and I am pretty sure I am using it correctly.
I am confused again.
Thanks
Shawn

Shawna · 2013-01-31 17:45

Ok more testing is done, I think I am having problems with my ATAN2 equation. Jason in post #5 you gave me a list of ATAN2 functions and results I tested them and here are my results, I am not sure where to go here.
ARoll := FF32.ATan2(Zero,NOne) The result was 0
ARoll := FF32.ATan2(NOne,Zero) The result was -1.570796
ARoll := FF32.ATan2(One,Zero) The result was 1.570796
ARoll := FF32.ATan2(Zero,One) The result was 0
I set up Zero, NOne, and One as constants. This is how I did that.
NOne = -1.0
Zero = 0.0
One = 1.0
I am lost again.

Shawna · 2013-01-31 18:09

Here is my test program for ATAN2

CON        _clkmode = xtal1 + pll16x
        _xinfreq = 5_000_000
  MHZ    = 80
  Zero    = 0.0
  One     = 1.0
  NOne    = -1.0
OBJ
  FF32      :"Float32Full"
  Debug    : "Extended_FDSerial"


VAR
  Long Stack[30]
  Long ARoll
  Long DBG_Out[30]
  


PUB Main
  
  FF32.Start                    'Starts Math Engine FF32


  cognew(Debugger, @Stack[0])   'Starts Debugger Function in new Cog
  waitcnt(clkfreq*4+cnt)        'Waits 4 seconds to let things initialize.


Repeat
    'ARoll  := FF32.ATan2(Zero,NOne) '= 0
    'ARoll  := FF32.ATan2(NOne,Zero) '= -1.570796
    ARoll  := FF32.ATan2(One,Zero) '= 1.570796
     'ARoll  := FF32.ATan2(Zero,One) = 0


    DBG_Out[18] := ARoll
    
PUB Debugger | x , idx
  Debug.start(1,2,0,115200)
  Debug.rxflush
  Dira[3] := 1
  Outa[3] := 0
  repeat    
    x:=Debug.rxtime(100)       
    if x=="a"
      repeat   idx from 0 to 119          ' 3 was 79
        Outa[3] := 1
        Debug.tx(byte[@DBG_Out+idx])
        Outa[3] := 0
      x:=-1

Shawna · 2013-02-01 13:01

So I was playing with a ATAN2 calculator online and I have determined that either I do not know how to us the Floating Point Full object or the object has a bug in it. I remember reading in another post that the ATAN2 function had a problem but I figured it had been fixed by now. Its probably my fault, but I am not getting the right values out of the function.

Thanks
Shawn

JasonDorie · 2013-02-01 13:30

The results of ATan2 are coming out in radians, where 1/2 PI = 1.57.. = 90 degrees.

Use:

CON
  RadToDeg = (180.0 / 3.141592654)

  Result := FF32.ATan2(X,Z);
  Result := FF32.FMul( Result , RadToDeg );

You also don't need to pass the inputs to ATan2 within a specific range - it'll work just fine without scaling the X & Z values by 0.004.

So, restating your results:
ARoll := FF32.ATan2(Zero,NOne) The result was 0 radians, or zero degrees (this is the only one I find weird - it should've been +/- 3.14159)
ARoll := FF32.ATan2(NOne,Zero) The result was -1.570796 radians, or -90 degrees
ARoll := FF32.ATan2(One,Zero) The result was 1.570796 radians, or +90 degrees
ARoll := FF32.ATan2(Zero,One) The result was 0 radians, or 0 degrees

I wrote the following in a C# app, and here's what I get:
r = Math.Atan2( 0.0f, -1.0f); = 3.14159
r = Math.Atan2(-1.0f, 0.0f); = -1.5707
r = Math.Atan2( 1.0f, 0.0f); = 1.5707
r = Math.Atan2( 0.0f, 1.0f); = 0.0

It's possible that whatever you're using to debug the output is messing it up. You could also try scaling the output and converting to an int, like this:

  Result := FF32.ATan2( Zero , NOne );
  Result := FF32.FMul( Result , RadToDeg );
  Result := FF32.FRound( Result );

Just send the resulting value to a serial terminal, because now it'll be an integer number from -180 to +180 degrees and will print fine.

Shawna · 2013-02-01 19:48

I am really missing something or the ATAN2 function or the ff32 object is messed up.
The following two statements are coming up with the same value. Its like the ATAN2 cannot deal with the negative one.

'ARoll := FF32.ATan2(Zero,NOne) '= 0
ARoll := FF32.ATan2(Zero,One) '= 0
ARoll := FF32.Degrees(ARoll)
ARoll := FF32.FTrunc(ARoll)
pst.Clear
pst.Dec(ARoll)

Zero = 0.0 as a constant
NOne = -1.0 as a constant
One = 1.0 as a constant
PST is the parallax serial terminal
WTF!

Shawna · 2013-02-02 15:39

OK,
I am almost positive the FF32 object is messed up for the ATAN2 function (ATan2(A,

). It appears to me that the function can not handle a zero for A.
Is there a way to use the ATAN function to get ATAN2 since ATAN only takes care of half the quadrants. I am stuck for sure now I do not know how to find the angles without ATAN2. How are other people doing this? People are using gyros and accelerometers in all sorts of projects.
Thanks
Shawn

Shawna · 2013-02-04 05:08

First off I would like to apologise for saying the float 32 object had an error in it. I still do not understand some of the results I was getting with the atan2 functions. But I realize that I don't really have enough experience to say there was a problem.
I think I was pressing to hard and got frustrated. After reading back through this thread many times and doing some googling I realized that I was over complicating the formulas.
I am now getting 0 degrees when the board is flat. If I start to rotate 1 axis the value goes from 0 to 180 degree. If I rotate the board further my readings go from -180 to 0 degrees and my board is flat again. I think this is the right result.
I will post the simple code if anyone is interested.
Thanks
Shown

prof_braino · 2013-02-04 06:27

Hey Shawna

Thanks for posting your questions and logging your path of discovery. Every time you ask a question, there are probably a dozen other folks with a similar question that appreciate the answer. This information is going to be helpful when I get to this point. On behalf of the lurkers on this thread, let me say again thanks, and keep up the good work.

Filtering Sensor Readings (Kalman Filter)

Comments