×

Efficient orientation estimation system using magnetic, angular rate, and gravity sensors

  • US 9,588,598 B2
  • Filed: 09/24/2015
  • Issued: 03/07/2017
  • Est. Priority Date: 06/30/2015
  • Status: Active Grant
First Claim
Patent Images

1. An efficient orientation estimation system using magnetic, angular rate, and gravity sensors, comprisinga three axis magnetometer configured to be attached to an object, wherein said three axis magnetometer generates magnetic field data represented in a sensor reference frame;

  • a three axis rate gyroscope configured to be attached to said object, wherein said three axis rate gyroscope generates angular velocity data represented in said sensor reference frame;

    a three axis accelerometer configured to be attached to said object, wherein said three axis accelerometer generates acceleration data represented in said sensor reference frame, wherein said acceleration data includes a gravity measurement;

    at least one processor comprising a tangible memory medium, coupled to said three axis accelerometer, to said three axis rate gyroscope, and to said three axis magnetometer, and configured todefine an Earth reference frame for an area in which said object is located, comprisinga unit vector ĵ

    pointing vertically upwards and opposite to a direction of gravity;

    a unit vector {circumflex over (k)} in a horizontal plane perpendicular to said unit vector ĵ

    , and pointing in a direction opposite to a horizontal component of Earth'"'"'s magnetic field in said area;

    a unit vector î

    perpendicular to said unit vector ĵ and

    perpendicular to said unit vector {circumflex over (k)};

    wherein said unit vector î

    , said unit vector ĵ

    , and said unit vector {circumflex over (k)} form a right-handed reference frame;

    define a sensor orientation as a rotation that rotates a vector in said sensor reference frame into a corresponding vector in said Earth reference frame;

    set said sensor orientation to an initial sensor orientation;

    iteratively calculate an updated value of said sensor orientation asobtain said angular velocity data from said three axis rate gyroscope;

    obtain said acceleration data from said three axis accelerometer;

    normalize said acceleration data to form a normalized acceleration having its vector length equal to one;

    obtain said magnetic field data from said three axis magnetometer;

    generate an Earth to sensor rotation as an inverse of said sensor orientation;

    rotate said unit vector ĵ

    by said Earth to sensor rotation, to form a gravity prediction in said sensor reference frame;

    apply said Earth to sensor rotation to a negative of said unit vector {circumflex over (k)}, to form a horizontal magnetic field prediction in said sensor reference frame;

    project said magnetic field data onto said normalized acceleration, to form a vertical magnetic component;

    subtract said vertical magnetic component from said magnetic field data, to form a horizontal magnetic component;

    normalize said horizontal magnetic component to form a normalized horizontal magnetic component;

    calculate a vector cross product of said normalized acceleration with said gravity prediction, to form a gravity errorcalculate a vector cross product of said normalized horizontal magnetic component with said horizontal magnetic field prediction, to form a magnetic field error;

    calculate an orientation derivative based onsaid angular velocity,said gravity error, andsaid magnetic field error;

    integrate said orientation derivative to form said updated value of said sensor orientation.

View all claims
  • 3 Assignments
Timeline View
Assignment View
    ×
    ×