mag_extendedkalman.h

很好的kalman程序。.zip文件包含了所有的用到kalman滤波的库和头文件。

//=============================================================================
/**
 *
 * @file MAG_ExtendedKalman.h
 * 
 * @author Christopher Konvalin <[e-mail]>
 * @version 4.0.0.1
 *
 * @bug 4/15/08 - CJK:
 * Modified quaternion to Euler conversions to a single standard quat2Euler 
 * (see \link mems::MAG_ExtendedKalman::quat2Euler \endlink)
 * function that corresponds to that given by Kuipers ("Quaternions and Rotation 
 * Sequences, a Primer with Applications to Orbits, Aerospace, and Virtual Reality", 
 * Princeton University Press, Princeton, 2002).
 * 
 * @bug 9/13/06 - CJK: Mantis 31\n
 * Reported originally by two separate customers, verified by CJK.  When 
 * calling quat2Euler the Euler angles are not correct. One customer identified 
 * this by orienting a nIMU such that the -Z axis was pointing magnetic 
 * north (+Z into chest), the -X (cable end) pointing down to earth. 
 * When simply bending forward 90 deg at the waist, then back up, the 
 * resulting Euler angles were incorrect. This failure was duplicated by 
 * MEMSense with a 1200 deg/s 5G nIMU.  Bug was related to incompatible
 * coordinate systems between literature and MEMSense (primarily right-
 * hand rotation and z-axis orientation; literature z-axis is 'up', 
 * MEMSense z-axis is 'down', into earth).  As a result, complete 
 * derivation of rotation matrices and equations from first priciples
 * was performed, with singularities at +/- PI/2 identified.  Both
 * 'earth-global' 
 * (see mems::MAG_ExtendedKalman::quat2EulerGlobal)
 * <br><br><b>4/15/08 - CJK NOTE: function quat2EulerGlobal no longer available - 
 * replaced by mems::MAG_ExtendedKalman::quat2Euler</b><br><br>
 * and 'device-local' 
 * (see mems::MAG_ExtendedKalman::quat2EulerLocal)
 * <br><br><b>4/15/08 - CJK NOTE: function quat2EulerLocal no longer available - 
 * replaced by mems::MAG_ExtendedKalman::quat2Euler</b><br><br>
 * Euler angles are now available.
 * As a result of offering both 'local' and 'global' euler angles, 
 * it is no longer appropriate to differentiate between 
 * processSampleQuat and processSampleEuler - thus the processing of
 * sample data was reduced to two function calls via 
 * processSample (overloaded with two different argument lists), which  
 * return a quaternion from which the user may then decide the appropriate 
 * Euler conversion.\n
 *
 * @bug 6/14/06 - CJK: 
 * An error in the matrix invert() method which, in very rare cases, would
 * produce out-of-bounds floating point values.  This would, in turn, produce
 * incorrect quaternion values and, as a result, corrupt the output.  Fixed.
 *
 * @bug 6/15/06 - CJK: 
 * If a resolution of zero (0) was passed in to the processSample method
 * of the Kalman, an exception would be thrown.  This has been replaced with
 * an internal check to verify the resolution is > 0.  If resolution == 0
 * then the filter ignores the input and returns the values calculated from 
 * the last valid sample.
 *
 *
 * @internal
 * 
 * The following is only necessary on VC++.NET when compiling the Kalman
 * library, itself.  End users of the library need not concern themselves
 * with the following build note.
 *
 * @note
 * Using the BOOST libraries/headers.  On VC++ an error will be generated
 * with the default settings, similar to "Internal heap limit reached".  The
 * workaround, as documented at http://www.boost.org/doc/html/variant.misc.html
 * is to increase the heap limit as follows:
 *
 * @par
 * "The compiler option /ZmNNN can increase the memory allocation limit. The NNN is 
 * a scaling percentage (i.e., 100 denotes the default limit). (Try /Zm200.)".  
 *
 * @par
 * Based on this recommendation, the compiler command line has the following
 * property set: /Zm200.  Appears to resolve the error.
 */
//=============================================================================
#ifndef MAG_EXTENDED_KALMAN_H
#define MAG_EXTENDED_KALMAN_H

#include "ConfigSTL.h"

namespace mems
{

/*********************************************
 * FORWARD DECLARATIONS                      *
 *********************************************/

/**
 * @internal
 *
 * Forward declaration of Extended Kalman implementation class.
 *
 * This class effectively hides the implementation details from the end user,
 * as well as greatly simplifies the compile-time dependencies due to external
 * header files.
 */
class MAG_ExtendedKalmanImpl;

/** 
 * 
 * Implementation class of Extended Kalman Filter.
 *
 * This class, presents an interface to the extended Kalman filter.  
 * Processing of a sample is invoked via one of the processSample methods,
 * each of which takes the resolution/difference between samples, and the
 * actual sample itself.  The sample consists of gyro x/y/z, accelerometer
 * x/y/z and magnetometer x/y/z; stated formally it is <gx, gy, gz, ax, ay, az,
 * mx, my, mz>.  The return value is a vector consisting of corrected gyro
 * values, a quaternion indicating the 'position' relative to canonical earth
 * coordinate axes (explained later), and gyro bias.  Again, formally stated
 * the result is <gx, gy, gz, qw, qx, qy, qz, bx, by, bz>.
 *
 * Two frames of reference, or coordinate systems, are used when discussing
 * operation of the filter.  The earth frame is fixed, and is aligned as 
 * follows: the positive x-axis points true north, the y-axis points east,
 * and the z-axis points 'down' toward the earth.  The body frame moves 
 * with the body being rotated/moved, and if using an airplane as the
 * reference the coordinate system can visualized as follows: the positive
 * x-axis (roll) points along the fuselage toward the cockpit, the positive 
 * y-axis (pitch) points down the right wing, and the positive z-axis (yaw)
 * points out the bottom of the aircraft.  The frames of reference are
 * established such that if the body is synchronized with the earth, the
 * two coordinate systems are perfectly aligned - earth x, y and z match
 * and align with body x, y and z.
 *
 * Various parameter tuning parameters can be set via the setProcessNoise and 
 * setMeasurementNoise methods.  Measurement noise is defined as the 
 * variance between measured samples.  Process noise is much more abstract,
 * but can be thought of as the noise external to the system (e.g. vibration,
 * shock, wind, etc.) that may in turn affect the quality of the measurement.
 * The current implementation of the filter attempts to account for changing
 * measurement noise characteristics by updating the measurement noise 
 * covariance matrix after each sample.  Future implementations will also
 * update the process noise covariance matrix in a similar manner.
 * 
 * Discussion of process and measurement noise can be found at the following:
 *      - http://www.andrew.cmu.edu/user/nsg/filter_parameters_theory.htm
 *      - http://www.cs.unc.edu/~welch/kalman/
 *
 * @par Initialization
 * For the filter to perform well, at least one initialization method must first
 * be called prior to library use:
 * 
 * @par
 * setEarthMag(): The Earth's magnetic field varies dramatically by location.  As 
 * such, the components of the magnetic field at your location must be set
 * prior to using the filter - the default values will likely be very different
 * from your location.
 *
 * @par Usage
 * processSample() is called with each new sample.  processSample() is overloaded
 * such that either the individual gyro, accel and magnetometer values may be 
 * passed in, or combined into a single vector.  Please see the individual methods
 * for additional information, including return values.
 * 
 * @internal
 * @see MAG_ExtendedKalmanImpl
 */
class MAG_ExtendedKalman
{
public:
    /**
     * 'Do-nothing' constructor.
     */
    MAG_ExtendedKalman();

    /**
     * Destructor
     */
    ~MAG_ExtendedKalman(void);

    /**
     * Returns library version.  Note that the version returned by this method
     * will contain additional release numbers indicating release and/or build
     * sequences that will not be present in the library (Kalmanxxx.lib) name.
     **/
    static const char * getVersion();

    /**
     * Reinitialize the filter to NULL state.  Reinitialization will reset the
     * process and measurement noise covariance matrices to their respective 
     * default values, and all other internal vectors/matrices to default,
     * initial values.
     **/
    void reinitialize();

    /**
     * Set the initial a priori estimate of the measurement.
     * Not necessary under normal conditions, but may be used
     * to decrease time to filter convergence, if required.
     * NOTE: Filter convergence is typically on the 
     * order of milliseconds assuming a standard IMU sample 
     * rate of 150Hz.
     *
     * @param p gyro x angular rate in deg/s
     * @param q gyro y angular rate in deg/s
     * @param r gyro z angular rate in deg/s
     * @param w quaternion w
     * @param x quaternion x
     * @param y quaternion y
     * @param z quaternion z
     */
    void setInitialEstimate(double p, double q, double r, 
        double w, double x, double y, double z);

    /**
     * Set the standard/canonical earth accelerations in G's.  Under standard
     * conditions - and preset by application upon startup - the +z-axis 
     * is +1.0, +x and +y are 0.0.  May be used if unusual acceleration 
     * effects are seen, or standard <0, 0, 1.0> acceleration is atypical
     * for the environment in which the filter is deployed.
     *
     * @param ax x-component acceleration specified in G's
     * @param ay y-component acceleration specified in G's
     * @param az z-component acceleration specified in G's
     */
    void setEarthAccel(double ax, double ay, double az);

    /**
     * Set the standard/canonical earth magnetic field in Gauss such that 
     * x is magnetic north, y is east, and z is toward center of earth. This
     * value is dependent upon latitude/longitude/elevation of use.  The
     * canonical values can be determined via two different methods:
     * 
     * -# The NOAA geomagnetic field calculator, which can be found here:
     * http://www.ngdc.noaa.gov/seg/geomag/magfield.shtml \n
     * First, determine your lat/lon/elev, which can be automatically determined
     * by the calculator based on Zip Code.  Once lat/lon/elevation is known,
     * select the 'Compute Magnetic Field Values' button and use the results from 
     * North/East/Vertical, which match up exactly with the filters canonical 
     * Earth  X/Y/Z .  The NOAA calculator gives magnetic field strength in nT
     * (nano Tesla) which is 1e-9, where 10000 Gauss = 1 Tesla.  Therefore, 
     * (nT * 1e-9 T/nT) * 10000 G/T gives Gauss.
     * -# Set the device flat on a table such that the positive x-axis is
     * pointing approximately magnetic north.  While collecting data, rotate 
     * the device repeatedly through magnetic north until the maximum magnetic