mag_extendedkalman.h

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

     * x-value is attained.  The x-axis and corresponding y and z-axis 
     * magnetic data can then be used as the values for the Kalman magnetic
     * x, y and z values.  It's important to make sure that, again, the data
     * is specified in Gauss.
     *
     *
     * @param mx magnetic x-component specified in Gauss
     * @param my magnetic y-component specified in Gauss
     * @param mz magnetic z-component specified in Gauss
     */
    void setEarthMag(double mx, double my, double mz);

    /**
     * Set the process noise matrix.  <b>The process noise is specified in
     * terms of variance, *not* standard deviation.</b>\n
     * NOTE: the process noise covariance matrix is updated after each sample
     * is processed.  A call to setProcessNoise will completely reset the
     * covariance matrix such that the specified noise values are along the
     * diagonal, and the remaining values are zero.
     *
     * @param gx gyro x-component 
     * @param gy gyro y-component 
     * @param gz gyro z-component 
     * @param qw quaternion w-component 
     * @param qx quaternion x-component 
     * @param qy quaternion y-component
     * @param qz quaternion z-component
     *
     * @see setMeasurementNoise
     */
    void setProcessNoise(double gx, double gy, double gz, double qw, 
        double qx, double qy, double qz);

    /**
     * Set the process noise matrix.  The order is <gx, gy, gz, qw, qx, qy, qz,
     * bx, by, bz>.  <b>The process noise is specified in terms of variance, *not*
     * standard deviation.</b>\n
     * NOTE: the process noise covariance matrix is updated after each sample
     * is processed.  A call to setProcessNoise will completely reset the
     * covariance matrix such that the specified noise values are along the
     * diagonal, and the remaining values are zero.
     *
     * @param noise vector process noise parameters that will be
     * placed along the diagonal of the process noise matrix.
     *
     * @see setMeasurementNoise
     * @see MEMS_STD_VECTOR
     */
    void setProcessNoise(MEMS_STD_VECTOR & noise);

    /**
     * Returns the current process noise covariance matrix.
     *
     * @return The process noise covariance matrix
     *
     * @see MEMS_STD_MATRIX
     */
    MEMS_STD_MATRIX getProcessNoiseCovariance();

    /**
     * Set the measurement noise for gyro, accelerometer and magnetometer.  
     * These values are generally empirically determined and  
     * <b>must be specified as a variance, not standard deviation.</b>\n
     * NOTE: the measurement noise covariance matrix is updated after each 
     * sample is processed.  A call to setMeasurementNoise will completely 
     * reset the covariance matrix such that the specified noise values are 
     * along the diagonal, and the remaining values are zero.
     *
     * @param gx gyro x-component in rad/s
     * @param gy gyro y-component in rad/s
     * @param gz gyro z-component in rad/s
     * @param ax accelerometer x-component in G's
     * @param ay accelerometer y-component in G's
     * @param az accelerometer z-component in G's
     * @param mx magnetometer x-component in Gauss
     * @param my magnetometer y-component in Gauss
     * @param mz magnetometer z-component in Gauss
     *
     * @see setProcessNoise()
     */
    void setMeasurementNoise(double gx, double gy, double gz, double ax, 
        double ay, double az, double mx, double my, double mz);

    /**
     * Set the measurement noise for gyro, accelerometer and magnetometer.  
     * These values are generally empirically determined and <b>must be 
     * specified as a variance, not standard deviation.</b> Order of parameters
     * is <gx, gy, gz, ax, ay, az, mx, my, mz> where g = gyro, 
     * a = accelerometer and m = magnetometer.\n
     * NOTE: the measurement noise covariance matrix is updated after each 
     * sample is processed.  A call to setMeasurementNoise will completely 
     * reset the covariance matrix such that the specified noise values are 
     * along the diagonal, and the remaining values are zero.
     *
     * @param noise vector of measurement variances for each sensor
     *
     * @see setProcessNoise()
     * @see MEMS_STD_VECTOR
     */
    void setMeasurementNoise(MEMS_STD_VECTOR & noise);

    /**
     * Returns the current measurement noise covariance matrix.
     *
     * @return The measurement noise covariance matrix
     *
     * @see MEMS_STD_MATRIX
     */
    MEMS_STD_MATRIX getMeasurementNoiseCovariance();

    /**
     * Process a sample of data and return the position data in quaterion
     * format.  The data sample is expected to be ordered as follows: 
     * <gx, gy, gz, ax, ay, az, mx, my, mz>.  The resolution of
     * the sample delta is the relative time between the current sample
     * and the previous sample in seconds.  The sample must be in the 
     * following units:
     *      - Gyro: rad/s
     *      - Accelerometer: G's
     *      - Magnetometer: Gauss
     *
     * @param delta time, in seconds, between the previous sample and the current sample
     * @param sample data sample in units ordered as: <gx, gy, gz, ax, ay, az, mx, my, mz>
     * @return vector ordered as follows: <gx, gy, gz, qw, qx, qy, qz, bx, by, bz> where
     * g = gyro in rad/s, q = normalized quaternion, b = gyro bias in rad.  
     *
     * @see MEMS_STD_VECTOR
     */
    MEMS_STD_VECTOR processSample(double delta, 
        MEMS_STD_VECTOR & sample);

    /**
     * Process a sample of data and return the position data in quaterion
     * format.  The resolution of the sample delta is the relative time 
     * between the current sample and the previous sample in seconds.  
     * The sample must be in the following units:
     *      - Gyro: rad/s
     *      - Accelerometer: G's
     *      - Magnetometer: Gauss
     *
     * @param delta time, in seconds, between the previous sample and the 
     * current sample
     * @param gx x-gyro angular rate (rad/s)
     * @param gy y-gyro angular rate (rad/s)
     * @param gz z-gyro angular rate (rad/s)
     * @param ax x-accelerometer (G)
     * @param ay y-accelerometer (G)
     * @param az z-accelerometer (G)
     * @param mx x-magnetometer (Gauss)
     * @param my y-magnetometer (Gauss)
     * @param mz z-magnetometer (Gauss)
     *
     * @return vector ordered as follows: 
     * <gx, gy, gz, qw, qx, qy, qz, bx, by, bz> where g = gyro in rad/s, 
     * q = normalized quaternion, b = gyro bias in rad. 
     *
     * @see MEMS_STD_VECTOR
     */
    MEMS_STD_VECTOR processSample(
        double delta, 
        double gx, double gy, double gz, 
        double ax, double ay, double az, 
        double mx, double my, double mz);

    /**
     * Returns the residual, or measurement innovation, which is defined as 
     * the discrepancy between the predicted measurement (x_hat_minus) and
     * the actual measurement (z).  A residual of zero means that the two
     * are in complete agreement (see Welch/Bishop - 
     * Introduction to Kalman Filter).  A 'zero' residual means the filter
     * is predicting perfectly the next step in the state transition and
     * that the filter is performing at its peak.
     *
     * @return The residual which is as follows: 
     * <gx, gy, gz, qw, qx, qy, qz> where g = gyro in rad/s,
     * q = normalized quaternion.
     *
     * @see MEMS_STD_VECTOR
     */
    MEMS_STD_VECTOR getResidual();

	/**
     * Normalized quaternion to Euler angle conversion.  The euler angles are 
     * in the order in which they are to be applied: yaw(z-axis), 
     * pitch(y-axis), roll(x-axis) in the LOCAL coordinate system (CS).  In the 
     * local CS the CS rotates with the device, and each new rotation is perfomed
     * relative to new orientation of the coordinate system - not the earth-based
     * coordinate system.  An example would be: 
     * starting in the canonical position where device x/y/z matches with earth
     * x/y/z, a +90 degree z-axis (yaw) rotation followed by a +90 degree rotation around
     * the x-axis (roll) produces a device orientation such that the longitudinal
     * axis (device x-axis) will be oriented east-west, or along the earth
     * y axis, and on its side.  If this same sequence was perfomed using a 
     * earth-based/global CS, the device will be oriented such that the device x-axis 
     * is aligned with the earth z-axis (north-south).\n
     * NOTE: The quaternion <b>must</b> be normalized.
     *
     * @param w quaternion w
     * @param x quaternion x
     * @param y quaternion y
     * @param z quaternion z
     * 
     * @return 3-element vector of euler angles, in radians, in the order in which 
     * they are to be applied (yaw, pitch, roll).
     *
     * @see MEMS_STD_VECTOR
     */
    MEMS_STD_VECTOR quat2Euler(double w, double x, double y, double z);



private:

    enum E_Quat
    {
        QW = 3,
        QX,
        QY,
        QZ
    };

    enum E_Euler
    {
        E_YAW = 0,
        E_PITCH,
        E_ROLL
    };

    static const char * VERSION;

    MAG_ExtendedKalmanImpl * getImpl();

    MAG_ExtendedKalmanImpl * m_impl;    // underlying implementation class
};

}

#endif // MAG_EXTENDED_KALMAN_H