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

Go to the documentation of this file.

/*
 * Ins.cc
 * Copyright (C) Mansi Shah 2009 <mrshah@users.sourceforge.net>
 *
 * Ins.cc 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.cc 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/>.
 */

#include "Ins.h"
#include <iostream>
#include <iomanip>
#include <string>
#include <time.h>
#include <cmath>

using namespace boost::numeric::ublas;

namespace uvsim
{

// Constructor
Ins::Ins(std::string accelCalib, std::string gyroCalib, std::string magCalib,
         double frequency, Imu6DofSensor * imu):
        accelM(3), accelB(3), gyroM(3), gyroB(3), magM(3), magB(3),
        gyro(3), quatdot(4), quatdot0(4), accel(3), mag(3), quat0(4), accelInertial0(3),
        accelInertial(3), velocity(3), velocity0(3), specificForce(3),
        mag0(3), accel0(3), gyro0(3),
        quat(4), euler(3), W(4,4), DCM(3,3), freq(frequency), imu(imu)
{
    std::cout << "Constructing Ins" << std::endl;

    // local variables
    std::ifstream accelFile, gyroFile, magFile;

    // open files
    accelFile.open(accelCalib.c_str());
    gyroFile.open(gyroCalib.c_str());
    magFile.open(magCalib.c_str());

    // read in coefficients
    accelFile >> accelM(0) >> accelB(0) >> accelM(1) >> accelB(1) >> accelM(2) >> accelB(2);
    gyroFile >> gyroM(0) >> gyroB(0) >> gyroM(1) >> gyroB(1) >> gyroM(2) >> gyroB(2);
    magFile >> magM(0) >> magB(0) >> magM(1) >> magB(1) >> magM(2) >> magB(2);

    // close files
    accelFile.close();
    gyroFile.close();
    magFile.close();

    // initialize magnetic model
    geoMag = new GeoMag(std::string(DATADIR)+="/data/WMM.COF",12);

    // start the imu
    imu->start(freq);

    std::cout << "Ins Constructed" << std::endl;
}

void Ins::initializeStatic(double alt, double lat, double lon)
{
    std::cout << "Performing Static Initialization" << std::endl;

    // initiliaze position
    altitudeInit = alt;
    latitudeInit = lat*M_PI/180;
    longitudeInit = lon*M_PI/180;

    altitude = altitudeInit;
    latitude = latitudeInit;
    longitude = longitudeInit;

    // initialize all derivatives to zero since static initialization
    altitudedot = 0;
    latitudedot = 0;
    longitudedot = 0;
    quatdot = zero_vector<double>(4);
    accelInertial = zero_vector<double>(3);
    velocity = zero_vector<double>(3);

    // calculate statistics
    sum_mag = sum_accel = sum_gyro = zero_vector<double>(3);
    sumSq_mag= sumSq_accel= sumSq_gyro = zero_vector<double>(3);
    avg_mag= avg_accel= avg_gyro = zero_vector<double>(3);
    std_mag= std_accel= std_gyro = zero_vector<double>(3);

    // compute sum and sum of square for data
    for (int j=0;j<initCycles;j++)
    {
        // read data from imu and apply calibrations
        read();

        // sums
        sum_mag=sum_mag+mag;
        sum_accel=sum_accel+accel;
        sum_gyro=sum_gyro+gyro;
        sumSq_mag=element_prod(mag,mag)+sumSq_mag;
        sumSq_accel=element_prod(accel,accel)+sumSq_accel;
        sumSq_gyro=element_prod(gyro,gyro)+sumSq_gyro;
    }

    // find averages
    avg_mag = sum_mag*(1./initCycles);
    avg_accel = sum_accel*(1./initCycles);
    avg_gyro = sum_gyro*(1./initCycles);

    // find standard deviations
    for (int i=0;i<3;i++)
    {
        std_mag(i)=sqrt((sumSq_mag(i)/initCycles)-(avg_mag(i)*avg_mag(i)));
        std_accel(i)=sqrt((sumSq_accel(i)/initCycles)-(avg_accel(i)*avg_accel(i)));
        std_gyro(i)=sqrt((sumSq_gyro(i)/initCycles)-(avg_gyro(i)*avg_gyro(i)));
    }

    // output statistics
    std::cout << "average mag: " << avg_mag << std::endl;
    std::cout << "average accel: " << avg_accel << std::endl;
    std::cout << "average gyro: " << avg_gyro << std::endl;

    std::cout << "stdDev mag: " << std_mag << std::endl;
    std::cout << "stdDev accel: " << std_accel << std::endl;
    std::cout << "stdDev gyro: " << std_gyro << std::endl;

    // set biases for gyros based on initialization
    gyroB = gyroB - (180/M_PI)*avg_gyro;

    // variables for rotations
    vector<double> axis0(3), axis1(3), quat0(4), quat1(4),
    magTempFrame(3), x(3), z(3);
    double angle0, angle1;

    // unit vectors
    z(0) = 0;
    z(1) = 0;
    z(2) = 1;
    x(0) = 1;
    x(1) = 0;
    x(2) = 0;

    // rotation 0, alignment with gravity vector
    axis0 = norm(crossProd(-avg_accel, z));
    angle0 = acos(inner_prod(-norm(avg_accel), z));
    quat0 = axisAngle2Quat(axis0, angle0);
    std::cout << "axis 0: " << axis0 << std::endl;
    std::cout << "angle 0 (deg): " << angle0*180/M_PI << std::endl;
    std::cout << "quat 0: " << quat0 << std::endl;
    std::cout << "eul 0: " << quat2Euler(quat0)*180/M_PI << std::endl;

    // rotation 1, alignment with magnetic field
    geoMag->update(altitude, 180/M_PI*latitude, 180/M_PI*longitude,magYear);

    magTempFrame = quatRotate(quat0, avg_mag);
    magTempFrame(2) = 0; // project to the NE frame
    magTempFrame = norm(magTempFrame);
    axis1 = norm(crossProd(magTempFrame, x));
    std::cout << "magTempFrameProj: " << magTempFrame << std::endl;
    angle1 = acos(inner_prod(x, magTempFrame)) + M_PI/180*geoMag->dec;
    quat1 = axisAngle2Quat(axis1, angle1);
    std::cout << "axis 1: " << axis1 << std::endl;
    std::cout << "angle 1 (deg): " << angle1*180/M_PI << std::endl;
    std::cout << "quat 1: " << quat1 << std::endl;
    std::cout << "eul 1: " << quat2Euler(quat1)*180/M_PI << std::endl;

    // initialize quaternions
    quat = quatProd(quat1,quat0);
    quat = norm(quat);
    std::cout << "quat: " << quat << std::endl;
    std::cout << "roll, pitch, yaw (deg): " << quat2Euler(quat)*180/M_PI << std::endl;

    // get initial time
    gettimeofday(&timeInitial , NULL);
    timePres = timeInitial;

    // initialize measurements
    read();

    std::cout << "Static Initialization Complete" << std::endl;
}

void Ins::read()
{
    // read the new imu values
    imu->update();

    // Calculate gyro and accel data from raw input
    mag = element_prod(magM, imu->mb) + magB;
    accel = (element_prod(accelM, imu->fb) + accelB)*g;
    gyro = (element_prod(gyroM, imu->wb) + gyroB)*M_PI/180;

    // set previous state as last step state
    accel0 = accel;
    mag0 = mag;
    gyro0 = gyro;

    quat0 = quat;
    quatdot0 = quatdot;

    latitude0 = latitude;
    longitude0 = longitude;
    altitude0 = altitude;

    latitudedot0 = latitudedot;
    longitudedot0 = longitudedot;
    altitudedot0 = altitudedot;

    velocity0 = velocity;
    accelInertial0 = accelInertial;
    timePrev = timePres;
}

void Ins::update()
{
    // update time (hard coded in contsructor
    gettimeofday(&timePres , NULL);

    // read data
    read();

    // To zero values for testing
    //accel = zero_vector<double>(3);
    //gyro = zero_vector<double>(3);

    // quaternion derivative matrix
    W(1,0) = W(2,3) = gyro(0);
    W(2,0) = W(3,1) = gyro(1);
    W(2,1) = W(0,3) = -gyro(2);
    W(3,0) = W(1,2) = gyro(2);
    W(3,2) = W(0,1) = -gyro(0);
    W(0,2) = W(1,3) = -gyro(1);
    W(0,0) = W(1,1) = W(2,2) = W(3,3) = 0;

    // quaternion derivative
    quatdot = 0.5*prod(W,quat0);

    // integrate quaternion
    quat = integrate(quatdot,quatdot0, quat0, freq);

    // normalize quaternion
    quat = norm(quat);

        // calculate euler angles
        euler = quat2Euler(quat);

    // update dcm matrix
    DCM(0,0) = pow(quat(0),2) + pow(quat(1),2) - pow(quat(2),2) - pow(quat(3),2);
    DCM(0,1) = 2 * (quat(1) * quat(2) - quat(0) * quat(3));
    DCM(0,2) = 2 * (quat(1) * quat(3) + quat(0) * quat(2));
    DCM(1,0) = 2 * (quat(1) * quat(2) + quat(0) * quat(3));
    DCM(1,1) = pow(quat(0),2) - pow(quat(1),2) + pow(quat(2),2) - pow(quat(3),2);
    DCM(1,2) = 2 * (quat(2) * quat(3) - quat(0) * quat(1));
    DCM(2,0) = 2 * (quat(1) * quat(3) - quat(0) * quat(2));
    DCM(2,1) = 2 * (quat(2) * quat(3) + quat(0) * quat(1));
    DCM(2,2) = pow(quat(0),2) - pow(quat(1),2) - pow(quat(2),2) + pow(quat(3),2);

    // computer specifice force in navigation frame
    specificForce = prod(DCM, accel);

    // Ldot , ldot , hdot
    latitudedot = velocity0(0) / (R0 + altitude0);
    longitudedot = velocity0(1) / (cos(latitude0) * (R0 + altitude0));
    altitudedot = -velocity0(2);

    // L, l, h
    latitude = integrate(latitudedot, latitudedot0, latitude0, freq);
    longitude = integrate(longitudedot, longitudedot0, longitude0, freq);
    altitude = integrate(altitudedot , altitudedot0, altitude0, freq);

    // Acceleration from specific force
    accelInertial(0) = specificForce(0) - velocity0(1) * (2*omega +
                       longitudedot) * sin (latitude) + velocity0(2) * latitudedot;
    accelInertial(1) = specificForce(1) + velocity0(0) * (2 * omega
                       + longitudedot) * sin (latitude) + velocity0(2) * (2 * omega +
                               longitudedot) * cos (latitude);
    accelInertial(2) = specificForce(2) - velocity0(1) * (2 * omega +
                       longitudedot) * cos (latitude) - velocity0(0) * latitudedot + g;

    // Integrate to get velocity
    velocity = integrate(accelInertial, accelInertial0, velocity0, freq);

    // Update magnetic model
    geoMag->update(altitude, 180/M_PI*latitude, 180/M_PI*longitude,magYear);
}

// Print
void Ins::print()
{
    // Print
    std::cout << std::setprecision(5);
    std::cout << "\nTime Elapsed (s): " << elapsedTime(timeInitial, timePres) << std::endl;
    std::cout << "Frequency (Hz): " << freq << std::endl;
    std::cout << "Latitude (deg): " << latitude*180/M_PI << std::endl;
    std::cout << "Longitude (deg): " << longitude*180/M_PI << std::endl;
    std::cout << "Altitude (m): " << altitude << std::endl;
    std::cout << "Velocity (m/s): " << velocity << std::endl;
    std::cout << "Quat (): " << quat << std::endl;
    std::cout << "Euler (deg)- Roll: "<< euler(0)*180/M_PI <<"  Pitch: " << euler(1)*180/M_PI <<"  Yaw: " <<euler(2)*180/M_PI << std::endl;
    std::cout << "Accel (m/s^2): " << accel << std::endl;
    std::cout << "Accel Inertial (m/s^2): " << accelInertial << std::endl;
    std::cout << "Roll Rates (deg/s): " << gyro*180/M_PI << std::endl;
    std::cout << "Accel M and B: " << accelM << " " << accelB  << std::endl;
    std::cout << "Gyro M and B: " << gyroM << " " << gyroB << std::endl;

    std::cout << "Latitude Arclength (m): " << (R0 + altitude)*
              (latitude-latitudeInit)  << std::endl;
    std::cout << "Longitude Arclength (m): " << (R0 + altitude)*
              (longitude-longitudeInit)  << std::endl;
    std::cout << "Altitude Change (m): " << (altitude-altitudeInit)  << std::endl;
    std::cout << "Distance Approx (m): " <<
              sqrt( pow((R0 + altitude)*(latitude-latitudeInit),2) +
                    pow((R0 + altitude)*(longitude-longitudeInit),2) +
                    pow(altitude - altitudeInit,2)) << std::endl;
    std::cout << "mag: " << mag << std::endl;
    std::cout << "mag heading in body frame: " << 180/M_PI*atan2(mag(1),mag(0)) << std::endl;
    std::cout << "mag inclination in body frame: " << 180/M_PI*atan2(mag(2), sqrt(mag(0)*mag(0)+mag(1)*mag(1))) << std::endl;
    std::cout << "dec: " << geoMag->dec << " dip: " << geoMag->dip << " ti: "
              << geoMag->ti << " gv: " << std::endl;
    imu->print();
    std::cout<<std::endl;
}



void Ins::stop()
{
    imu->stop();
}

//Destruct r
Ins::~Ins()
{
    stop();
}


}

// vim:ts=4:sw=4

---