/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/navigation/Ins.h

Go to the documentation of this file.

/*
 * Ins.h
 * Copyright (C) Mansi Shah 2009 <mrshah@purdue.edu>
 *
 * Ins.h is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Ins.h is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef Ins_H
#define Ins_H

#include <ctime>
#include <string>
#include <fstream>
#include <vector>
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/storage_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include "Navigator.h"
#include "uvsim/sensing/Imu6DofSensor.h"
#include "uvsim/navigation/GeoMag.h"
#include "uvsim/guidance/GeoCoord.h"
#include "uvsim/utilities/utilities.h"
#include "uvsim/utilities/constants.h"

using namespace boost::numeric::ublas;

namespace uvsim
{

// Constants
class Ins
{
public:

    // Variables
    double latitude, longitude, altitude;
    double latitude0, longitude0, altitude0;
    double latitudedot, longitudedot, altitudedot;
    double latitudedot0, longitudedot0, altitudedot0;
    double latitudeInit, longitudeInit, altitudeInit;
    vector<double> accelInertial, accelInertial0, specificForce,
    velocity, velocity0, accel, gyro, mag, quatdot, quatdot0,
    quat, quat0, euler, accel0, gyro0, mag0;
    double freq;
    matrix<double> W, DCM;
    timeval timeInitial, timePrev, timePres;
    vector<double> accelM, accelB, gyroM, gyroB,
    magM, magB;
    Imu6DofSensor * imu;
        
        // Constants
        static const int initCycles = 2000; // number of cycles for initialization
        static const double magYear = 2009; // year for magnetic model to use

    //Averages for bias calculation
    vector<double> sum_mag, sum_accel, sum_gyro,
    sumSq_mag, sumSq_accel, sumSq_gyro,
    avg_mag, avg_accel, avg_gyro,
    std_mag, std_accel, std_gyro;

    // Earth Geomagnetic Model
    GeoMag * geoMag;

    // Constructor
    Ins(std::string accelCalib, std::string gyroCalib, std::string magCalib,
        double frequency, Imu6DofSensor * imu);

    // Initialize
    void initializeStatic(double altitude0, double latitudae0, double longitude0);

    // read the imu and apply calibrations
    void read();

    // Update Function
    void update();

    // Print Function
    void print();

    // Stop Function
    void stop();

    // Accessor Functions
    vector<double> getQuat()
    {
        return quat;
    }
    GeoCoord getGeo()
    {
        return GeoCoord(longitude, latitude, altitude);
    }

    // Destructor
    virtual ~Ins();

};

}

#endif
// vim:ts=4:sw=4

---