6DOF IMU remains in room for about an hour
cessnapilot
Posts: 182
Hi,
I attached the FPU code of a 200 Hz 6DOF IMU made from 4 H48C 3D accelerometers. This along with the code
··· http://obex.parallax.com/objects/335/ INS Math WGS84·Earth
made it possible to build a cheap, gyro-free unit.·During assembly and calibration I used the
··· http://obex.parallax.com/objects/464/ General MEMS· Sensor Model
object intensively. See attached documents for details.
Using 5 minutes of initialization and then continuous internal drift and temperature compensation, this simple device can "keep" its ECEF position within a 5x5x3 m room either during rest or during continuous movement of its carrier inside the room usually for about an hour. Considering that an average H48C would drift away about a million km during that time, this is not so bad.
Such precision will allow us, with an additional Propeller,·to build a GPS NMEA sentence cleaner, where GPS NMEA sentences of noisy data are coming in irregularly and corrected NMEA sentences are going out regularly, describing position with much higher frequency and accuracy. For this signal cleaner we can use· the
··· http://obex.parallax.com/objects/405/·Gps Float
object to capture NMEA sentences without missing a single one.
Or, if we have a digital map of the roads, this precision allows us to construct a satellite-free GPS module for our car. The only inconvenience is that we have to know our starting position on the map. This map an be scrolled with a color display using the
http://obex.parallax.com/objects/418/·GPS Scrolling Maps
object with an additional Propeller, too.
I am collecting now experiences with an other 6DOF IMU sensor, 4 sensors as usual, but this time with 3· LISY300 single-axis gyro and one H48C 3D accelerometer.
Cheers
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
I attached the FPU code of a 200 Hz 6DOF IMU made from 4 H48C 3D accelerometers. This along with the code
··· http://obex.parallax.com/objects/335/ INS Math WGS84·Earth
made it possible to build a cheap, gyro-free unit.·During assembly and calibration I used the
··· http://obex.parallax.com/objects/464/ General MEMS· Sensor Model
object intensively. See attached documents for details.
Using 5 minutes of initialization and then continuous internal drift and temperature compensation, this simple device can "keep" its ECEF position within a 5x5x3 m room either during rest or during continuous movement of its carrier inside the room usually for about an hour. Considering that an average H48C would drift away about a million km during that time, this is not so bad.
Such precision will allow us, with an additional Propeller,·to build a GPS NMEA sentence cleaner, where GPS NMEA sentences of noisy data are coming in irregularly and corrected NMEA sentences are going out regularly, describing position with much higher frequency and accuracy. For this signal cleaner we can use· the
··· http://obex.parallax.com/objects/405/·Gps Float
object to capture NMEA sentences without missing a single one.
Or, if we have a digital map of the roads, this precision allows us to construct a satellite-free GPS module for our car. The only inconvenience is that we have to know our starting position on the map. This map an be scrolled with a color display using the
http://obex.parallax.com/objects/418/·GPS Scrolling Maps
object with an additional Propeller, too.
I am collecting now experiences with an other 6DOF IMU sensor, 4 sensors as usual, but this time with 3· LISY300 single-axis gyro and one H48C 3D accelerometer.
Cheers
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
Comments
-Jack
It would be good to catch up with that. I guess that within 3-5 years most of the cars will have INS units, similar grade that of your friend's one. The fabricated device is a prototype and needed patience, hard work and many hours for the calibration. Fortunately almost every step of calibration can (should)· be automated with a CNC robot in a thermal chamber.
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
Beautifully clear exposition. Thank you for sharing.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Let it sit idle for a bit on each side on a perfectly level surface to calibrate for gravity and any inaccuracy between sensor placement.
I have a large CNC router so I could run it back and forth at different known speeds to calibrate the translational axes.
The best that I could come up with for rotations without making some sort of custom jig was to place it on a record player and let it spin a bit while counting revolutions. The results between my count and its count could be compared to make it more accurate. That could be done at different speeds.
What was your calibration procedure? Also, I'm curious about why you chose to put the sensors on two boards spaced apart from each other rather than on just one board. I would think that just having them rigidly mounted in relation to each other would do the trick.
BTW the gyro (ITG-3200) is 3 axis and has an I2C interface. Cost is about $15-20 and Sparkfun now have chips and a pcb available. I still have not completed my design - trying to fit it into a small plastic box for protection. I am looking at the Hammond 1551 boxes. Here is the link to my thread http://forums.parallax.com/forums/default.aspx?f=25&p=001&m=436063
The gyro, accelerometer, compass and pressure only takes approx 1.2"x1.2". About the same for the prop and microSD (for datalogging). A large part of the pcb is the 3pin connections for the RC Receiver & the servos/escs - 2 sets of 3x8 pin headers.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Links to other interesting threads:
· Home of the MultiBladeProps: TriBlade,·RamBlade,·SixBlade, website
· Single Board Computer:·3 Propeller ICs·and a·TriBladeProp board (ZiCog Z80 Emulator)
· Prop Tools under Development or Completed (Index)
· Emulators: CPUs Z80 etc; Micros Altair etc;· Terminals·VT100 etc; (Index) ZiCog (Z80) , MoCog (6809)·
· Prop OS: SphinxOS·, PropDos , PropCmd··· Search the Propeller forums·(uses advanced Google search)
My cruising website is: ·www.bluemagic.biz·· MultiBlade Props: www.cluso.bluemagic.biz
The methods you mentioned use integration of the primary measured values. Velocity comes from acceleration and rotation count comes from angular acceleration/angular velocity. It's OK, but does not make life easier. I suggest to calibrate primary quantities first. For example, you can reach the same velocity with many ways of varying acceleration (then which of those values do you calibrate?), and you measure the same acceleration (g) with a resting sensor as with one at constant velocity. It is much simpler to keep it at rest. However, measuring speed and rotation count can be used to test algorithms and verify primary calibration.
My method is described in the text of PDT attachment. It uses steady poses and rotations at steady angular rates. Crucial is to keep the temperature stable.
Three sensors are always in the same plane, wherever you place them, but the 4th one should not be in that plane to have a real 3D array of them. It's a good idea to mount three of them rigidly on a PCB, but the 4th one should be placed above or below that plane.
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
This is a truly amazing piece of work. How far did you travel with the unit in the hour?· This could be a serious addition to the navigation the teams use for the DARPA challenges with the atonomus ground vehicle.· When do you think you will have some results on the gyro version?
RS-JIM
The ECEF X, Y data was written into EEPROM for about 50 minutes in each second, from that the path was somewhat more than 1 km. Now testing with a car shows good accuracy, as well. On the field the device has usually good days and sometime bad days. Even on bad days, it deviates less than 100 m after a ~40 km round trip in town.
The gyro version is under development with· temperature stabilization within 45-55 degrees of Celsius with at least 2 celsius precision at the chips. The whole assembly will have thermal insulation. At that temperatures I hope it will only dissipate heat into the surrounding, and because of the insulation, the waste heat of the four sensors will accumulate effectively in the Al heat sink/bath to allow for only a small amount of extra thermostat heating. Having many other things to do, I guess 3 months are necessary for the first calibrated results.
Cheers,
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
I'm impressed by the low amount of drift that you've achieved.· I've used a 2D gyro to control a gimaballed rocket motor, and I found that drift was a big problem. · Fortunately, the rocket motor burns for only a few seconds, so the gyro worked OK for my application.· I minimized the drift by callibrating the average value from the gyros for several seconds just before liftoff.· I used a 12-bit ADC, but I found that I needed about 15 bits for the average value.· The signal noise allowed me to compute the 15-bit average.
I also found that sometimes the gyro worked quite well, and sometimes it drifted off very fast.· I believe this is due to dead-zones created by using only 12 bits for the ADC.· If the noise were very low the ADC would produce a constant value, and there is no way to measure the true average value to more than 12 bits of resolution.· A half-bit error at 12 bits would accumulate to full range in 80 seconds if it were sampled at 100 times per second.
Do you think your system could be applied to a free-space IMU, or do you require a 1G long-term reference to maintain calibration?
Dave
The calibration is maintained by recognition of special motion states, like
> Very close to 1G acceleration and close to zero angular acceleration
-->Then constant velocity and constant rotation around the motion direction is assumed
-->Case of no rotation checked
---->Then temperature correction factor is updated
---->Angular acceleration drift and some (or all) of the gyro drift is corrected
---->Calculated attitude is updated with the measured values of the accelerations and from the magnetometer data
---->Calculated quaternions are updated accordingly
The "calibration useful" motion states and the method of updates depend on the vehicle. For example, cars full stop sometimes or airplanes cruise during seconds leveled with constant heading or make coordinated turn, etc... For a car we can assume at yaw update no sideslip, etc...
On the long term, beside the temperature correction, we have to adjust the G reference, as well. It is not easy to separate their effect in the General Sensor Model math, so an independent temperature measurement or stabilization is necessary to achieve that.
With the General Sensor Model in the free-space instead of the 1G reference you can use 0G long-term reference when motors are switched off and solar wind is negligible. In that motion state gyro-drift can be controlled assuming constant angular impulse, then strapped down angular velocity components nicely follow Euler equations and, I suppose, calculated and measured rotations can be compared to eliminate drift of the gyros.
It is interesting, that you can control "inertially" the attitude of the spacecraft without sending mass into space with rockets or boosters, by slowing down or speeding up rotating masses inside the ship.
I'm just wandering that some cleverly placed small masses (1-2kg each) rotating at very high speed can inertially stabilize an F1 car to have less air drag than otherwise?
Cheers,
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
I hadn't realized you had a magnetometer.· I can see that the magnetic reference could be used with the 1G reference to·determine the absolute attitude, which would compensate for any gyro drift.· It's also interesting how the physics of the vehicle would be used to control drift as well.
My use of the term "free space" may not be correct.· I think what I should have said was "unconstrained motion" instead.· I am interested in developing an IMU for use in rockets.· In this situation, gravity cannot be used as a reference.· As an example, a rocket could be dropped from a high altitude balloon, and a rocket motor could be used to accelerate it at 1G.· In this situation there is no way determine the attitude without using gyros as an inertial reference.· If the gyros drift, then the reference will drift as well.
I think the solution is to use a magnetometer like you use.· In addition, a horizon detector and maybe a solar position detector could be used to callibrate the attitude.
Dave
A magnetometer and Word Magnetic Model 2010 is used. The magnetometer is Honeywell made but the WMM2010 calculations are done with a Prop + FPU combination. The WMM2010 provides the complete geometry of the Earth magnetic field from 1 km below the Earth’s surface to 850 km above the surface. However, instead of the EGM96 15-Minute Geoid Height File (~10MByte where geoid approximates MSL) I use a lower resolution ten-by-ten degree geoid height data (that fits into the Prop + FPU) to approximate MSL on the Earth. The calculated magnetic field vector for a given date, MSL height, latitude, longitude is practically (5 figures or more) the same as with the giant dataset of geoid heights. Not to mention that the usual accuracy of magnetometers are less than those 5 figures. In summary: a Propeller + FPU combination can calculate the WMM2010 magnetic data for magnetometer applications. I plan to make and use a HM55B triad based upon the dual driver in OBEX, where the Prop, beside the WMM2010 stuff, can deal with the sensors, as well. All these will be published in the INSGPSMAGAIR series with the codes posted or placed in OBEX, as usual.
For your rocket attitude determination I guess a magnetometer may be sufficient when the rocket changes its attitude relatively slowly, as magnetometer readouts are not very fast.·
Cheers,
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
And similar is true for the gravitation. Whichever body axis is parallel wth G, the corresponding angle (Yaw for Z, Pitch for Y and Roll for X) can not be determined from the measured single(!) G data.
I suggest 2 solutions·for this problem
Solution 1: Do not care about it. It is rare to have such alignments for a long time in the real life. So, if you forget about that problem, it might walk away by itself. Write a robust "Yaw, Pitch and Roll from Bx, By, Bz" calculating routine, that does not hang up in such, almost never existing cases. And that's all. During that short time of the never existing axis coincidences (when one or two·of the B components are zero), the routine even could output critical values by extrapolating from the previously measured results.
Solution 2: This needs some more bucks. Mount another magnetometer on the rocket, but rotate the second one with a precisely KNOWN angle relative to the first one. None of the axis of the two strapped down sensor should be parallel. You can bet, that there will be no such aligment of the rocket where BOTH sensors are ineffective. This solution is so simple, that it is even not published, as far as I know. Well, I admit, that it is not easy to make a PHd from it, but it is surely patented by some 'inventors'. So, be careful of lawyers·in this case.
Cheers,
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
I would like to ask you a couple of questions.
You said you used WMM2010, and you had to calculate MSL from geoid altitude.
The maximum difference between MSL and geoid is about 100m. Will this affect the WWM2010 data?
Moreover some gps units are offering a MSL altitude. Could it help and reduce calculation overhead? For instance I used a Sanav unit (FV-M8), and the MSL data in my area are rather good. On the other hand I used it on a sailing boat, and the unit didn't update speed and course if speed was below 4-5 knots.... so in my case it was rather useless...
Massimo
For us who walk, sail or fly it is convenient to define a reference for zero elevation---usually called mean sea level (MSL). As far as I know, the geoid means MSL if you allow some practical simplifications. The actual separation between the mean sea level and the calculated geoid should only be a few meters (<3m) as geoid is intended only(!) to reproduce MSL. This difference is not so important and therefore we can used these terms as synonyms (This is correct·if we know that they are different, one is the real thing the other is an accurate math approximation of it). And here is an official definition: the US National Geodetic Survey (NGS) defines the geoid as “the equipotential surface of the earth’s gravity field that best fits, in a least squares sense, global mean sea level”. Period.
What you mentioned is probably the undulations of the MSL (or geoid, use what you like better)· with respect to the WGS84 ellipsoid. The sea surface has its own permanent “hills and valleys”, although much smoother and smaller than those found on land masses. For example, the Earth’s rotation about its polar axis generates a centrifugal force that causes the oceans to accumulate, or bulge, around the equator. Moreover, the density of magma is uneven in the earth’s crust. Areas of greater density exert stronger gravitational pull causing the accumulation of water and permanently raising the MSL. The undulations of MSL have a maximun of 192 meters. The biggest anomaly was discovered southeast of India where the geoid (or MSL if you like) is 105 meters below the WGS84 ellipsoid, and its highest swelling was observed in eastern Indonesia as 87 m.
To answer your first question: The WMM2010 calculation begins with the calculation of the altitude abowe the WGS84 ellipsoid.·Instead of querying that directly, it is more practical to ask the user the altitude above MSL,··which he can understand, perceive and measure easily.· If I were a sailor and my GPS would ask me to enter my altitude above the WGS84 ellipsoid, I would throw the unit immediately overboard. So, the user gives the data relative to the MSL,·but the unit has to know the altitude of that MSL above or below the WGS84 ellipsoid. An that cames· from·the geoid data·table·with some interpolation.·Finally, on a skipper MSL altitude on the bridge should be always positive and·a small(!) more or less constant value.
I know·of GPS·units (e.g. SONY pixel) that gives velocity data below 5 knots, although that may be not· very accurate to trim the ship in weak winds. On the sailing boats of my company's sailing club we use small speedometers on the larger ships, these meters·work·with·small propeller containing sensors mounted under the waterline. They are accurate at slow speeds, as well.
Cheers,
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
RE: Magnetometer usage
Unfortunately, CessnaPilot's Solution 1 and Solution 2 will not work.· It all goes back to a generalization of Hein's argument.· The generalization is this:
Imagine that your (3 axis) magnetometer is fixed in some coordinate system.· In that coordinate system, you will measure some x, y, and z magnetic strength values.· Use this vector as an axis of rotation·for the coordinate system.· Now, any rotation about this axis will not cause the measured x, y, and z values·to change.· What this means is that the "attitude" of the coordinate system cannot be computed from the x, y, and z magnetic strength values.· More accurately, it is possible to do a computation, but it will in general be wrong in that it will be only one of the infinitely many "attitudes" that are produced by rotations about the magnetic axis.
Confession: I did try to produce a equation to calculate attitude (at this point·I had belief in Solution 1 and was also counting on Solution 2).· I used quaternion math, and axis-angle math, and Euler math.· Each gave an answer.· Each was different.· It took a lot of testing to prove to myself that all the answers were correct.· Then, and only then did I arrive at the thought experiment given in the previous paragraph.
Solution 2 suffers from the same problem.· Now you simply have two sets of xyz magnetic readings that do not change.
I would love it if someone can prove me wrong about this.· If I am right, I also wish that someone had given such a clear explanation and saved me·a LOT of math fiddling.
Usually I'm pretty good about knowing when a problem can be solved.· In this case I was seduced by the three magnetic readings.· I knew that it takes three variables to describe "attitude" ( 3 Euler angles for example).· If you had said, here are 2 measurements, please calculate 3 variables, I would have said - sorry cannot do, the problem is under-constrained so there will be lots of answers.· But here I had three readings, so I charged ahead.
I feel a little silly about all this, but I wonder how many others have gone down the same path.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Post Edited (Bob Anderson) : 6/5/2010 6:03:26 PM GMT
Well, in my INS/GPS/magneto system I use the triad algorithm to find attitude from the G and B vectors. An example of this is placed on OBEX, in the Matrix_Driver_Demo object.
PST.Char(PST#CS) PST.Str(STRING("*************************************************")) PST.Char(PST#NL) PST.Str(STRING("Somewhere in the Troposphere of Mother Earth the")) PST.Char(PST#NL) PST.Str(STRING("navigation computer calculates the NED components")) PST.Char(PST#NL) PST.Str(STRING("of the magnetic and gravity vectors for that place:")) PST.Char(PST#NL) PST.Str(STRING(" B(North)[noparse][[/noparse]uT] = ")) PST.Str(FloatToString(magnNED[noparse][[/noparse]0], 51)) PST.Str(STRING(PST#NL, " B(East) [noparse][[/noparse]uT] = ")) PST.Str(FloatToString(magnNED[noparse][[/noparse]1], 51)) PST.Str(STRING(PST#NL, " B(Down) [noparse][[/noparse]uT] = ")) PST.Str(FloatToString(magnNED[noparse][[/noparse]2], 51)) PST.Str(STRING(PST#NL, "G(North)[noparse][[/noparse]m/s2] = ")) PST.Str(FloatToString(gravNED[noparse][[/noparse]0], 73)) PST.Str(STRING(PST#NL, "G(East) [noparse][[/noparse]m/s2] = ")) PST.Str(FloatToString(gravNED[noparse][[/noparse]1], 73)) PST.Str(STRING(PST#NL, "G(Down) [noparse][[/noparse]m/s2] = ")) PST.Str(FloatToString(gravNED[noparse][[/noparse]2], 73)) PST.Char(PST#NL) WAITCNT(12 * _DBGDEL + CNT) PST.Str(STRING("However, the strapped down sensors measure these")) PST.Char(PST#NL) PST.Str(STRING("vectors in Body frame components, and they are :")) PST.Str(STRING(PST#NL, " B(Nose) [noparse][[/noparse]uT] = ")) PST.Str(FloatToString(magnBody[noparse][[/noparse]0], 51)) PST.Str(STRING(PST#NL, " B(Rwing)[noparse][[/noparse]uT] = ")) PST.Str(FloatToString(magnBody[noparse][[/noparse]1], 51)) PST.Str(STRING(PST#NL, " B(Bely) [noparse][[/noparse]uT] = ")) PST.Str(FloatToString(magnBody[noparse][[/noparse]2], 51)) PST.Str(STRING(PST#NL, "G(Nose) [noparse][[/noparse]m/s2] = ")) PST.Str(FloatToString(gravBody[noparse][[/noparse]0], 73)) PST.Str(STRING(PST#NL, "G(Rwing)[noparse][[/noparse]m/s2] = ")) PST.Str(FloatToString(gravBody[noparse][[/noparse]1], 73)) PST.Str(STRING(PST#NL, "G(Bely) [noparse][[/noparse]m/s2] = ")) PST.Str(FloatToString(gravBody[noparse][[/noparse]2], 73)) PST.Char(PST#NL) WAITCNT(8 * _DBGDEL + CNT) PST.Str(STRING("What are the Heading, Pitch and Roll Euler angles")) PST.Char(PST#NL) PST.Str(STRING("of the airplane in a straightforward and accurate")) PST.Char(PST#NL) PST.Str(STRING("approximation? Let us use the 'Triad' algorithm.")) PST.Char(PST#NL) PST.Str(STRING("This algorithm may be good for your robot, as well.")) PST.Char(PST#NL) PST.Str(STRING("-------------------------------------------------->")) WAITCNT(6 * _DBGDEL + CNT) 'Let us use the Triad algorithm to calculate the Body to NED 'Direction Cosine Matrix (DCM) 'First create an orthogonal, rigth handed frame using the Body 'frame coordinates of the two measured physical vector. Let us 'start with the magnetic vector FPUMAT.Vector_Unitize(@t1b, @magnBody) FPUMAT.Vector_CrossProduct(@t2b, @magnBody, @gravBody) FPUMAT.Vector_Unitize(@t2b, @t2b) FPUMAT.Vector_CrossProduct(@t3b, @t1b, @t2b) 'Then create the same frame (the triad) but calculate it from the 'NED frame physical vector components this time FPUMAT.Vector_Unitize(@t1n, @magnNED) FPUMAT.Vector_CrossProduct(@t2n, @magnNED, @gravNED) FPUMAT.Vector_Unitize(@t2n, @t2n) FPUMAT.Vector_CrossProduct(@t3n, @t1n, @t2n) 'Unit vector DCM matrices can now be created from these othogonal 'tb, tn unit vectors by putting the vector components into the 'columns of [noparse][[/noparse]3x3] DCM matrices 'Create {Body to Triad} rotation matrix REPEAT i FROM 0 TO 2 dcmBT[noparse][[/noparse]i * 3] := t1b[noparse][[/noparse]i] dcmBT[noparse][[/noparse](i * 3) + 1] := t2b[noparse][[/noparse]i] dcmBT[noparse][[/noparse](i * 3) + 2] := t3b[noparse][[/noparse]i] 'Create {NAV to Triad} rotation matrix REPEAT i FROM 0 TO 2 dcmNT[noparse][[/noparse]i * 3] := t1n[noparse][[/noparse]i] dcmNT[noparse][[/noparse](i * 3) + 1] := t2n[noparse][[/noparse]i] dcmNT[noparse][[/noparse](i * 3) + 2] := t3n[noparse][[/noparse]i] 'Transpose this matrix to obtain {Triad to NAV} rotation matrix FPUMAT.Matrix_Transpose(@dcmTN, @dcmNT, 3, 3) 'Now we can calculate {Body to NAV} rotation matrix by multiplying '{Body to Triad} * {Triad to NAV} rotation matrices FPUMAT.Matrix_Multiply(@dcmBN, @dcmBT, @dcmTN, 3, 3, 3) 'Now we have the Body to NAV DCM. From this we can calculate 'an approximation to the attitude 'Psi = ArcTan(-DCM21/DCM11) fV := dcmBN[noparse][[/noparse]0] '(1-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) fV := dcmBN[noparse][[/noparse]3] '(2-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 127) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmd(FPUMAT#_FNEG) FPUMAT.WriteCmdByte(FPUMAT#_ATAN2, 126) FPUMAT.WriteCmd(FPUMAT#_DEGREES) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_FREADA) heading := FPUMAT.ReadReg 'Theta = ArcTan(DCM31/SQRT(DCM11^2+DCM21^2)) fV := dcmBN[noparse][[/noparse]0] '(1-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 125) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_FMUL, 125) fV := dcmBN[noparse][[/noparse]3] '(2-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_FMUL, 126) FPUMAT.WriteCmdByte(FPUMAT#_FADD, 125) FPUMAT.WriteCmd(FPUMAT#_SQRT) fV := dcmBN[noparse][[/noparse]6] '(3-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 127) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_ATAN2, 126) FPUMAT.WriteCmd(FPUMAT#_DEGREES) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_FREADA) pitch := FPUMAT.ReadReg 'Phi = ArcTan(DCM32/DCM33) fV := dcmBN[noparse][[/noparse]8] '(3-1)*3 + (3-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) fV := dcmBN[noparse][[/noparse]7] '(3-1)*3 + (2-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 127) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_ATAN2, 126) FPUMAT.WriteCmd(FPUMAT#_DEGREES) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_FREADA) roll := FPUMAT.ReadReg 'Now let us do it again starting with the gravity vector this time. 'Create an orthogonal, rigth handed frame using the two measured 'physical vector's Body frame coordinates FPUMAT.Vector_Unitize(@t1b, @gravBody) FPUMAT.Vector_CrossProduct(@t2b, @gravBody, @magnBody) FPUMAT.Vector_Unitize(@t2b, @t2b) FPUMAT.Vector_CrossProduct(@t3b, @t1b, @t2b) 'Then create the same frame (the triad) but now calculate with the 'NED frame physical vector components FPUMAT.Vector_Unitize(@t1n, @gravNED) FPUMAT.Vector_CrossProduct(@t2n, @gravNED, @magnNED) FPUMAT.Vector_Unitize(@t2n, @t2n) FPUMAT.Vector_CrossProduct(@t3n, @t1n, @t2n) 'Unit vector DCM matrices can now be created From these othogonal 'tb, tn unit vectors-by-putting the t vector components into the 'columns of [noparse][[/noparse]3x3] DCM matrices 'Create {Body to Triad} rotation matrix REPEAT i FROM 0 TO 2 dcmBT[noparse][[/noparse]i * 3] := t1b[noparse][[/noparse]i] dcmBT[noparse][[/noparse](i * 3) + 1] := t2b[noparse][[/noparse]i] dcmBT[noparse][[/noparse](i * 3) + 2] := t3b[noparse][[/noparse]i] 'Create {NAV to Triad} rotation matrix REPEAT i FROM 0 TO 2 dcmNT[noparse][[/noparse]i * 3] := t1n[noparse][[/noparse]i] dcmNT[noparse][[/noparse](i * 3) + 1] := t2n[noparse][[/noparse]i] dcmNT[noparse][[/noparse](i * 3) + 2] := t3n[noparse][[/noparse]i] 'Transpose it to obtain {Triad to NAV} rotation matrix FPUMAT.Matrix_Transpose(@dcmTN, @dcmNT, 3, 3) 'Now we can calculate {Body to NAV} rotation matrix by multiplying '{Body to Triad} * {Triad to NAV} rotation matrices FPUMAT.Matrix_Multiply(@dcmBN, @dcmBT, @dcmTN, 3, 3, 3) 'Again we have the Body to NAV DCM. From this we can calculate 'another approximation to the attitude 'Psi = ArcTan(-DCM21/DCM11) fV := dcmBN[noparse][[/noparse]0] '(1-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) fV := dcmBN[noparse][[/noparse]3] '(2-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 127) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmd(FPUMAT#_FNEG) FPUMAT.WriteCmdByte(FPUMAT#_ATAN2, 126) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_DEGREES) 'Take average with previous value FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, heading) FPUMAT.WriteCmdByte(FPUMAT#_FADD, 127) FPUMAT.WriteCmdByte(FPUMAT#_FDIVI, 2) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_FREADA) heading := FPUMAT.ReadReg 'Convert to compass bearing oKay := FPUMAT.Float_GT(0.0, heading, 0.0) IF oKay 'Then give 360 to heading FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 127) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, heading) FPUMAT.WriteCmdByte(FPUMAT#_FADDI, 120) FPUMAT.WriteCmdByte(FPUMAT#_FADDI, 120) FPUMAT.WriteCmdByte(FPUMAT#_FADDI, 120) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_FREADA) heading := FPUMAT.ReadReg 'Theta = ArcTan(DCM31/SQRT(DCM11^2+DCM21^2)) fV := dcmBN[noparse][[/noparse]0] '(1-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 125) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_FMUL, 125) fV := dcmBN[noparse][[/noparse]3] '(2-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_FMUL, 126) FPUMAT.WriteCmdByte(FPUMAT#_FADD, 125) FPUMAT.WriteCmd(FPUMAT#_SQRT) fV := dcmBN[noparse][[/noparse]6] '(3-1)*3 + (1-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 127) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_ATAN2, 126) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_DEGREES) 'Take average FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, pitch) FPUMAT.WriteCmdByte(FPUMAT#_FADD, 127) FPUMAT.WriteCmdByte(FPUMAT#_FDIVI, 2) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_FREADA) pitch := FPUMAT.ReadReg 'Phi = ArcTan(DCM32/DCM33) fV := dcmBN[noparse][[/noparse]8] '(3-1)*3 + (3-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) fV := dcmBN[noparse][[/noparse]7] '(3-1)*3 + (2-1) FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 127) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, fV) FPUMAT.WriteCmdByte(FPUMAT#_ATAN2, 126) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_DEGREES) 'Take average FPUMAT.WriteCmdByte(FPUMAT#_SELECTA, 126) FPUMAT.WriteCmdFloat(FPUMAT#_FWRITEA, roll) FPUMAT.WriteCmdByte(FPUMAT#_FADD, 127) FPUMAT.WriteCmdByte(FPUMAT#_FDIVI, 2) FPUMAT.Wait FPUMAT.WriteCmd(FPUMAT#_FREADA) roll := FPUMAT.ReadReg 'Display results PST.Str(STRING(PST#NL, "Heading = ")) PST.Str(FloatToString(heading, 50)) PST.Str(STRING(PST#NL, " Pitch = ")) PST.Str(FloatToString(pitch, 50)) PST.Str(STRING(PST#NL, " Roll = ")) PST.Str(FloatToString(roll, 50)) PST.Char(PST#NL)This works like charm.
Heins question was about to use magnetometers in rockets while not using G. I wrote that post based it on some (maybe too simple) logic of a pilot.
Method 1: Artificial horizon has only one gyroscope. A gyroscope keeps its direction in the inertial space.·Artificial horizons can indicate pitch and roll but yaw. Disregarding Earth's rotation, the magnetic field vector is a pretty accurate constant direction in the inertial space. So avoiding any math I guessed that with only one magnetometer one can find pitch and roll like an artificial horizon does. For a rocket, going up vertically that might be enough. Otherwise, if one has the initial yaw, math solution can be unique in time. This second statement that I really guess, even now.
Method 2: Beside artificial horizon there is directional gyro. A simple heading indicator contains one 'free' gyro. These two gyros (AI+DG) allow us to measure the full attitude of the aircraft. I only guessed that 2 magnetometers can substitute those 2 gyros. But now, I will find a math proof (or disproof) of that (and of method 1 with inital yaw data).
One more thing. 2 measurements really mean 6 data (B1x, ..., B2z) here.
Cheers,
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
Post Edited (cessnapilot) : 6/6/2010 10:58:58 AM GMT
Basically, any object that has zero average acceleration can use G for a reference.· This would include an object descending under a parachute or a balloon rising up through the atmosphere.· They achieve a zero-acceleration equilibrium where air-resistance equals the force acting upon them.
A rocket typically goes through three phases.· The first phase is when it is·accelerated upward under the motor thrust.· Then it goes through a coast phase, and finally it descends under a parachute back to the ground.· Gravity cannot be used as a reference during the first two phases because it cannot be seperated from the accelerations due to the other forces.
Normally the thrust and coast phases are just a few seconds, so gyro drift is not a problem.· However, I am looking into a multi-stage rocket where the coast phases would be several minutes.· That is why I am interested in a reference using a magnetometer and either the sun or an optical horizon detector as the other reference.
Dave
The amazing thing about a 3 axis magnetometer is that you cannot even determine where North is based only on the Bx By Bz values.· You must know something about the "attitude" of the magnetometer to turn the Bx...z readings into heading information.
I don't think that this is news to either Dave Hein or cessnapilot.· I wrote this up because I have seen posts where other people are expecting things from magnetometer readings that simply cannot be done, like·using one as·a subsitute for gyros and accelerometers for determining attitude.· I hope that the thought experiment I outlined will steer other experimenters away from·this particular ·dead-end and into more productive avenues of thought.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
You wrote
"The amazing thing about a 3 axis magnetometer is that you cannot even determine where North is based only on the Bx By Bz values.· You must know something about the "attitude" of the magnetometer to turn the Bx...z readings into heading information."
I will sketch a thought experiment. Which is the step that is impossible or the reasoning that is incorrect?
Step 0: One has a good magnetometer and he knows where he is (altitude above MSL, latitude, longitude). The question is where North is.
Step 1: One calculates the NED Bx, By, Bz (Bn, Be, Bd) magnetic field components with the WMM2010 procedure.
Step 2: One rotates in space the magnetometer that it shows exactly Bx, By and Bz readings. With sign, of course.
Step 3: There is one such position of that magnetometer.
Step 4: The magnetometer x axis then points to the geographic North (to the axis of Earth's rotation)
Step 5: The magnetometer's x, y plane is horizontal
Step 6: The vector Bx+By points to the Magnetic North
Step 7: One can draw a line onto the magnetometer's body that points along that vector, i.e. points to Magnetic North
I left my iPhone at the airfield, it has a built in magnetometer with graphic display, so I can't play with it this evening. I will be there on wendsday. Besides, I have a Honeywell HM3300 sensor. With that I will try to check the steps I listed.· Wether it works or not, I will post the result. If it works you have to reject a dogma. If it does not work, you are right.
Cheers,
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
You are right, and I was wrong with the "magnetometer only" method1 and method2. That is why one needs 2 independent physical vectors to figure out attitude or North.
To catch your point I did not even used a magnetometer. A rectangular shoe box did it. I rotated it around its 3D diagonal, then the penny dropped. Silly me, so I have to forget my dogma about 3D vectors. Thank you.
Cheers,
Istvan
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Intentionally Left Blank
Haven't heard anything more on this thread lately. Have you had success with the 3 gyros and one accelerameter approach?
RS_Jim