PhidgetSpatial - How to calculate bearing

Legacy support with Phidget21
markjoakim
Phidgetsian
Posts: 8
Joined: Wed Mar 03, 2010 9:32 am
Contact:

PhidgetSpatial - How to calculate bearing

Postby markjoakim » Tue Jun 29, 2010 8:07 am

Hi

I have been trying to make an AS3 example of how to get information from the compass on the PhidgetSpatial and turn that into a direction defined by somewhere between 0 and 360 degrees.. I based it off the C# example.. It currently outputs the right compass bearing, except for when I tilt the PhidgetSpatial to either side (pitchAngle in the code below).. Does anyone have any clue as to how I'd go about correcting the compass bearing by applying some mathematics relative to the pitch angle thing?

One other thing: I thought the C# example was already supposed to be doing just that..? Is it just because some of the code didn't work the same way in AS3?

And one last thing: In the piece of code I've provided below, I ended up just calling some phidgets functions to get the data I needed, because I wasn't able to figure out how to get the data from the event (evt:PhidgetDataEvent)..

Regards,
Mark M.

Here's the code I have so far (it should include everything from the C# example):

Code: Select all

import com.phidgets.*;
import com.phidgets.events.*;
import flash.geom.*

var compassBearingFilter:Array = new Array();
var compassBearingFilterSize:int = 10;
var compassBearing:Number = 0;
var lastBearing:Number = 0;

var phidSp:PhidgetSpatial = new PhidgetSpatial();

phidSp.addEventListener(PhidgetEvent.DETACH, onSpDetach);
phidSp.addEventListener(PhidgetEvent.ATTACH, onSpAttach);
phidSp.addEventListener(PhidgetErrorEvent.ERROR, onError);
phidSp.addEventListener(PhidgetDataEvent.SPATIAL_DATA, onSpData);

phidSp.open("localhost", 5001, null, 45176);

function startInst():void {
}

function onSpData(evt:PhidgetDataEvent):void {
   var Xh:Number = 0;
   var Yh:Number = 0;
   var gravity:Vector3D = new Vector3D(phidSp.getAcceleration(0), phidSp.getAcceleration(2), phidSp.getAcceleration(1));
   gravity.normalize();
   var pitchAngle:Number = Math.asin(gravity.x);
   var rollAngle:Number = Math.asin(gravity.z);
   if(gravity.y < 0) {
      pitchAngle = -pitchAngle;
      rollAngle = -rollAngle;
   }
   var xRotMatrix:Matrix3D = new Matrix3D();
   xRotMatrix.rawData = Vector.<Number>(
      [Math.cos(pitchAngle),Math.sin(pitchAngle),0,0,
       -Math.sin(pitchAngle),Math.cos(pitchAngle),0,0,
       0,0,1,0,
       0,0,0,1]);
   var zRotMatrix:Matrix3D = new Matrix3D();
   zRotMatrix.rawData = Vector.<Number>(
      [1,0,0,0,
       0,Math.cos(rollAngle),Math.sin(rollAngle),0,
       0,-Math.sin(rollAngle),Math.cos(rollAngle),0,
       0,0,0,1]);
   var xRotMCopy:Matrix3D = xRotMatrix.clone();
   xRotMCopy.append(zRotMatrix);
   var rotMatrix:Matrix3D = xRotMCopy.clone();
   var data:Vector3D = new Vector3D(phidSp.getMagneticField(0), phidSp.getMagneticField(2), -phidSp.getMagneticField(1));
   var dataCopy:Vector3D = data.clone();
   dataCopy.add(new Vector3D(rotMatrix.rawData[0], rotMatrix.rawData[1], rotMatrix.rawData[2], rotMatrix.rawData[3]));
   var correctedData:Vector3D = dataCopy.clone();
   Xh = -correctedData.z;
   Yh = -correctedData.x;
   try {
      var bearing:Number = 0;
      var _360inRads:Number = (360 * Math.PI / 180.0);
      if (Xh < 0) bearing = Math.PI - Math.atan(Yh / Xh);
      else if (Xh > 0 && Yh < 0) bearing = -Math.atan(Yh / Xh);
      else if (Xh > 0 && Yh > 0) bearing = Math.PI * 2 - Math.atan(Yh / Xh);
      else if (Xh == 0 && Yh < 0) bearing = Math.PI / 2.0;
      else if (Xh == 0 && Yh > 0) bearing = Math.PI * 1.5;
      //The board is up-side down
      if (gravity.y < 0) {
         bearing = Math.abs(bearing - _360inRads);
      }
      //passing the 0 <-> 360 point, need to make sure the filter never contains both values near 0 and values near 360 at the same time.
      var stuff:Array = new Array(new Number, new Number, new Number);
      if(Math.abs(bearing - lastBearing) > 2) { //2 radians == ~115 degrees
         if(bearing > lastBearing) {
            for each (stuff in compassBearingFilter)
               stuff[0] += _360inRads;
         } else {
            for each (stuff in compassBearingFilter)
               stuff[0] -= _360inRads;
         }
      }

      compassBearingFilter.push(new Array(bearing, pitchAngle, rollAngle));
      if (compassBearingFilter.length > compassBearingFilterSize)
         compassBearingFilter.shift();

      bearing = pitchAngle = rollAngle = 0;
      for each (stuff in compassBearingFilter) {
         bearing += stuff[0];
         pitchAngle += stuff[1];
         rollAngle += stuff[2];
      }
      bearing /= compassBearingFilter.length;
      pitchAngle /= compassBearingFilter.length;
      rollAngle /= compassBearingFilter.length;

      compassBearing = bearing * (180.0 / Math.PI);
      if(compassBearing > 360) compassBearing -= 360;
      else if(compassBearing < 0) compassBearing += 360;
      lastBearing = bearing;

      spatdata.text = String(compassBearing);
   } catch(errObject:Error) {}
}
function onError(evt:PhidgetErrorEvent):void {
   trace(evt);
}
function onSpAttach(evt:PhidgetEvent):void {
   phidSp.DataRate = 128;
   startInst();
}
function onSpDetach(evt:PhidgetEvent):void {
}

markjoakim
Phidgetsian
Posts: 8
Joined: Wed Mar 03, 2010 9:32 am
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby markjoakim » Tue Jun 29, 2010 8:09 am

I haven't studied mathematics for a long time.. Maybe some parts of the code above don't even make sense..

Robert

Re: PhidgetSpatial - How to calculate bearing

Postby Robert » Tue Jun 29, 2010 9:25 am

markjoakim wrote:I thought the C# example was already supposed to be doing just that..? Is it just because some of the code didn't work the same way in AS3?

Yes the C# code does do that. The reason being that we usually put most of the effort into the C# examples, as they are the ones included in the Windows Phidget control panel. Not all examples get treated with the same level of detail.

You could take a look at the C# code to see how it's calculated.

User avatar
Patrick
Lead Developer
Posts: 3155
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby Patrick » Tue Jun 29, 2010 9:40 am

I don't know anything about Matrixes/Vectors in AS3.0, but it appears that you would want to use the Matrix3D.deltaTransformVector() method for applying the rotation matrix to the compass vector. Also, some of your rotation matrix elements are reversed. Some (untested) code that should work:

Code: Select all

private function onSpatialData(evt:PhidgetDataEvent):void{
   var sData:PhidgetSpatialEventData = evt.Data as PhidgetSpatialEventData;

   ...

   var rotMatrix:Matrix3D = new Matrix3D();
   rotMatrix.rawData = Vector.<Number>(
      [Math.cos(pitchAngle),-Math.sin(pitchAngle),0,0,
      Math.sin(pitchAngle),Math.cos(pitchAngle),0,0,
      0,0,1,0,
      0,0,0,1]);

   var zRotMatrix:Matrix3D = new Matrix3D();
   zRotMatrix.rawData = Vector.<Number>(
      [1,0,0,0,
      0,Math.cos(rollAngle),-Math.sin(rollAngle),0,
      0,Math.sin(rollAngle),Math.cos(rollAngle),0,
      0,0,0,1]);

   rotMatrix.append(zRotMatrix);

   var data:Vector3D = new Vector3D(sData.MagneticField[0], sData.MagneticField[2], -sData.MagneticField[1]);
   var correctedData:Vector3D = rotMatrix.deltaTransformVector(data);

   Xh = -correctedData.z;
   Yh = -correctedData.x;

   ...
}

This also shows how to access the event data.

-Patrick

markjoakim
Phidgetsian
Posts: 8
Joined: Wed Mar 03, 2010 9:32 am
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby markjoakim » Tue Jun 29, 2010 11:28 am

patrick wrote:I don't know anything about Matrixes/Vectors in AS3.0, but it appears that you would want to use the Matrix3D.deltaTransformVector() method for applying the rotation matrix to the compass vector. Also, some of your rotation matrix elements are reversed. Some (untested) code that should work:
......

This also shows how to access the event data.

-Patrick


Hi :) Thanks for your reply.. I really need this to work using AS3, and I greatly appreciate your help.. I tried to implement your piece of code and make the changes you suggested.. My current code still outputs a number which changes relative to how you tip the board.. I hope it's just me who misunderstood how to implement the code.. My current code looks like this:

Code: Select all

function onSpData(evt:PhidgetDataEvent):void {
   var sData:PhidgetSpatialEventData = evt.Data as PhidgetSpatialEventData;
   var Xh:Number = 0;
   var Yh:Number = 0;
   var gravity:Vector3D = new Vector3D(sData.Acceleration[0], sData.Acceleration[2], sData.Acceleration[1]);
   gravity.normalize();
   var pitchAngle:Number = Math.asin(gravity.x);
   var rollAngle:Number = Math.asin(gravity.z);
   if(gravity.y < 0) {
      pitchAngle = -pitchAngle;
      rollAngle = -rollAngle;
   }
   var rotMatrix:Matrix3D = new Matrix3D();
   rotMatrix.rawData = Vector.<Number>(
      [Math.cos(pitchAngle),-Math.sin(pitchAngle),0,0,
       Math.sin(pitchAngle),Math.cos(pitchAngle),0,0,
       0,0,1,0,
       0,0,0,1]);
   var zRotMatrix:Matrix3D = new Matrix3D();
   zRotMatrix.rawData = Vector.<Number>(
      [1,0,0,0,
       0,Math.cos(rollAngle),-Math.sin(rollAngle),0,
       0,Math.sin(rollAngle),Math.cos(rollAngle),0,
       0,0,0,1]);

   rotMatrix.append(zRotMatrix);

   var data:Vector3D = new Vector3D(sData.MagneticField[0], sData.MagneticField[2], -sData.MagneticField[1]);
   var correctedData:Vector3D = rotMatrix.deltaTransformVector(data);

   Xh = -correctedData.z;
   Yh = -correctedData.x;
   try {
      var bearing:Number = 0;
      var _360inRads:Number = (360 * Math.PI / 180.0);
      if (Xh < 0) bearing = Math.PI - Math.atan(Yh / Xh);
      else if (Xh > 0 && Yh < 0) bearing = -Math.atan(Yh / Xh);
      else if (Xh > 0 && Yh > 0) bearing = Math.PI * 2 - Math.atan(Yh / Xh);
      else if (Xh == 0 && Yh < 0) bearing = Math.PI / 2.0;
      else if (Xh == 0 && Yh > 0) bearing = Math.PI * 1.5;
      //The board is up-side down
      if (gravity.y < 0) {
         bearing = Math.abs(bearing - _360inRads);
      }
      //passing the 0 <-> 360 point, need to make sure the filter never contains both values near 0 and values near 360 at the same time.
      var stuff:Array = new Array(new Number, new Number, new Number);
      if(Math.abs(bearing - lastBearing) > 2) { //2 radians == ~115 degrees
         if(bearing > lastBearing) {
            for each (stuff in compassBearingFilter)
               stuff[0] += _360inRads;
         } else {
            for each (stuff in compassBearingFilter)
               stuff[0] -= _360inRads;
         }
      }

      compassBearingFilter.push(new Array(bearing, pitchAngle, rollAngle));
      if (compassBearingFilter.length > compassBearingFilterSize)
         compassBearingFilter.shift();

      bearing = pitchAngle = rollAngle = 0;
      for each (stuff in compassBearingFilter) {
         bearing += stuff[0];
         pitchAngle += stuff[1];
         rollAngle += stuff[2];
      }
      bearing /= compassBearingFilter.length;
      pitchAngle /= compassBearingFilter.length;
      rollAngle /= compassBearingFilter.length;

      compassBearing = bearing * (180.0 / Math.PI);
      if(compassBearing > 360) compassBearing -= 360;
      else if(compassBearing < 0) compassBearing += 360;
      lastBearing = bearing;

      spatdata.text = "Grader: " + String(compassBearing);
   } catch(errObject:Error) {}
}


Maybe I should've left some more of the original code out..?

markjoakim
Phidgetsian
Posts: 8
Joined: Wed Mar 03, 2010 9:32 am
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby markjoakim » Tue Jun 29, 2010 12:51 pm

Finally! I changed a couple of things in the matrices, and now everything works.. This is the code:

Code: Select all

function onSpData(evt:PhidgetDataEvent):void {
   var sData:PhidgetSpatialEventData = evt.Data as PhidgetSpatialEventData;
   var Xh:Number = 0;
   var Yh:Number = 0;
   var gravity:Vector3D = new Vector3D(sData.Acceleration[0], sData.Acceleration[2], sData.Acceleration[1]);
   gravity.normalize();
   var pitchAngle:Number = Math.asin(gravity.x);
   var rollAngle:Number = Math.asin(gravity.z);
   if(gravity.y < 0) {
      pitchAngle = -pitchAngle;
      rollAngle = -rollAngle;
   }
   var rotMatrix:Matrix3D = new Matrix3D();
   rotMatrix.rawData = Vector.<Number>(
      [Math.cos(pitchAngle),Math.sin(pitchAngle),0,0,
       -Math.sin(pitchAngle),Math.cos(pitchAngle),0,0,
       0,0,1,0,
       0,0,0,1]);
   var zRotMatrix:Matrix3D = new Matrix3D();
   zRotMatrix.rawData = Vector.<Number>(
      [1,0,0,0,
       0,Math.cos(rollAngle),Math.sin(rollAngle),0,
       0,-Math.sin(rollAngle),Math.cos(rollAngle),0,
       0,0,0,1]);

   rotMatrix.append(zRotMatrix);

   var data:Vector3D = new Vector3D(sData.MagneticField[0], sData.MagneticField[2], -sData.MagneticField[1]);
   var correctedData:Vector3D = rotMatrix.deltaTransformVector(data);

   Xh = -correctedData.z;
   Yh = -correctedData.x;
   try {
      var bearing:Number = 0;
      var _360inRads:Number = (360 * Math.PI / 180.0);
      if (Xh < 0) bearing = Math.PI - Math.atan(Yh / Xh);
      else if (Xh > 0 && Yh < 0) bearing = -Math.atan(Yh / Xh);
      else if (Xh > 0 && Yh > 0) bearing = Math.PI * 2 - Math.atan(Yh / Xh);
      else if (Xh == 0 && Yh < 0) bearing = Math.PI / 2.0;
      else if (Xh == 0 && Yh > 0) bearing = Math.PI * 1.5;
      //The board is up-side down
      if (gravity.y < 0) {
         bearing = Math.abs(bearing - _360inRads);
      }
      //passing the 0 <-> 360 point, need to make sure the filter never contains both values near 0 and values near 360 at the same time.
      var stuff:Array = new Array(new Number, new Number, new Number);
      if(Math.abs(bearing - lastBearing) > 2) { //2 radians == ~115 degrees
         if(bearing > lastBearing) {
            for each (stuff in compassBearingFilter)
               stuff[0] += _360inRads;
         } else {
            for each (stuff in compassBearingFilter)
               stuff[0] -= _360inRads;
         }
      }

      compassBearingFilter.push(new Array(bearing, pitchAngle, rollAngle));
      if (compassBearingFilter.length > compassBearingFilterSize)
         compassBearingFilter.shift();

      bearing = pitchAngle = rollAngle = 0;
      for each (stuff in compassBearingFilter) {
         bearing += stuff[0];
         pitchAngle += stuff[1];
         rollAngle += stuff[2];
      }
      bearing /= compassBearingFilter.length;
      pitchAngle /= compassBearingFilter.length;
      rollAngle /= compassBearingFilter.length;

      compassBearing = bearing * (180.0 / Math.PI);
      if(compassBearing > 360) compassBearing -= 360;
      else if(compassBearing < 0) compassBearing += 360;
      lastBearing = bearing;

      spatdata.text = "Grader: " + String(compassBearing);
   } catch(errObject:Error) {}
}


I hope this helps other people as well :) And thanks again :) Maybe you could put an AS3 code sample file online based on this..

I have one other question related to calculating the compass bearing: The compass bearing sometimes shows the opposite direction and stays in that area until you shake the board or move it around a little.. Could there possibly be some way to counter this? It happens frequently and in the C# example app as well..

User avatar
Patrick
Lead Developer
Posts: 3155
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby Patrick » Tue Jun 29, 2010 1:55 pm

Glad it's working. As for the odd bearing - what are the compass vector components when it's getting messed up? If you hold the board with the magnetic field almost completely along the z-axis, you will not get very good data. Here in calgary, the magnetic field is only 15 degrees off vertical, so that's pretty easy to do.

Also, rotation matrices aren't always that great, a more robust algorithm would probably rotate the angles differently - this is mostly just an example kept as simple as possible, and it should generally work quite well, but you need to understand the underlying principles to see where things can go wrong.

-Patrick

markjoakim
Phidgetsian
Posts: 8
Joined: Wed Mar 03, 2010 9:32 am
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby markjoakim » Tue Jun 29, 2010 2:20 pm

patrick wrote:Glad it's working. As for the odd bearing - what are the compass vector components when it's getting messed up? If you hold the board with the magnetic field almost completely along the z-axis, you will not get very good data. Here in calgary, the magnetic field is only 15 degrees off vertical, so that's pretty easy to do.

Also, rotation matrices aren't always that great, a more robust algorithm would probably rotate the angles differently - this is mostly just an example kept as simple as possible, and it should generally work quite well, but you need to understand the underlying principles to see where things can go wrong.


In Denmark, where I am right now, the magnetic declination is exactly 0.. I personally don't know much about rotation matrices either.. Actually, I worked my way through the AS3 code by reading the Flash manual and replacing the C# code with stuff that seemed similar..

It's true that I don't understand the underlying principles thoroughly, but I was hoping that I would be able to make this work anyway. I just need to get this last small detail sorted out somehow.. Plus it's a little time critical, and I know more about programming than mathematics..

As to your first question, currently I just know that I'm keeping the board somewhat steady and flat and then turning it quickly from north to south or vice versa..

User avatar
Patrick
Lead Developer
Posts: 3155
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby Patrick » Tue Jun 29, 2010 3:46 pm

Declination is not important, it's the inclination. For example, in Copenhagen, the declination is 2 degrees and the inclination is 70 degrees, which means your field is only 20 degrees off vertical. The closer your magnetic vector is to the gravity vector (perpendicular to earth), the less accuracy you will get because only the horizontal components of the magnetic field vector are useful for determining bearing.

http://www.ngdc.noaa.gov/geomagmodels/IGRFWMM.jsp

At any rate, your Matrix3D object seems to have some methods for directly dealing with rotation - these may be better to use. Also, you may want to turn off the bearing filter while testing - this is really just there to smooth out the data so the visual output isn't jittery.

-Patrick

markjoakim
Phidgetsian
Posts: 8
Joined: Wed Mar 03, 2010 9:32 am
Contact:

Re: PhidgetSpatial - How to calculate bearing

Postby markjoakim » Tue Jun 29, 2010 4:05 pm

patrick wrote:Declination is not important, it's the inclination. For example, in Copenhagen, the declination is 2 degrees and the inclination is 70 degrees, which means your field is only 20 degrees off vertical. The closer your magnetic vector is to the gravity vector (perpendicular to earth), the less accuracy you will get because only the horizontal components of the magnetic field vector are useful for determining bearing.

http://www.ngdc.noaa.gov/geomagmodels/IGRFWMM.jsp

At any rate, your Matrix3D object seems to have some methods for directly dealing with rotation - these may be better to use. Also, you may want to turn off the bearing filter while testing - this is really just there to smooth out the data so the visual output isn't jittery.


Ah, so in short, Denmark is not the ideal place to use an electronical compass..?
It's usable in my current project, though. I'll just have to make my program aware of this stuff, I guess..

I appreciate your help :) I'll see if I can figure this out..


Return to “ActionScript 3.0 - Flash CS3 / Flex / AIR”

Who is online

Users browsing this forum: No registered users and 0 guests