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

Go to the documentation of this file.

/*
 * navigatorTest.cpp
 *
 *  Created on: Apr 28, 2009
 *      Author: jgoppert
 */

#include <iostream>
#include <string>
#include "uvsim/navigation/Ins.h"
#include "uvsim/navigation/GeoMag.h"
#include "uvsim/utilities/utilities.h"
#include "uvsim/visualization/Frame.h"
#include <osg/PositionAttitudeTransform>
#include <uvsim/navigation/KalmanFilter.h>
#include <uvsim/dynamics/AhrsErrorDynamics.h>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>

using namespace uvsim;
using namespace boost::numeric::ublas;

vector<double> measure(Ins * ins);

int main()
{
    // constants
    const static double insFreq = 500, dispFreq = 20, kfFreq = 300;

    // cutoff frequencies
    vector<double> freqCutMb(3), freqCutFb(3), freqCutWb(3);

    // magnetometer cutoff freq
    freqCutMb(0) = 312; //312;
    freqCutMb(1) = 312; //312;
    freqCutMb(2) = 312; //312;

    // accel cutoff freq
    freqCutFb(0) = 350; //350;
    freqCutFb(1) = 350; //350;
    freqCutFb(2) = 150; //150;

    // gyro cutoff freq
    freqCutWb(0) = 120; //120;
    freqCutWb(1) = 120; //120;
    freqCutWb(2) = 120; //120;

    // variables
    float dec, dip, ti, gv;

    // Purdue airport
    float alt, glat, glon, time;
    alt = 184.7; // m
    glat = 40.41194444; // deg
    glon = -86.93361111; // deg
    time = 2009.7; // decimal years

    // test ins
    Imu6DofSensor imu("/dev/rfcomm0", freqCutMb, freqCutFb, freqCutWb);
    Ins ins(std::string(DATADIR)+="/data/Ins15g.dat",
                   std::string(DATADIR)+="/data/Gyro.dat",
                   std::string(DATADIR)+="/data/Mag.dat",insFreq,&imu);
    ins.initializeStatic(alt,glat,glon);

        // test navigation
    int n = 9; // state size
    int m = 3; // measurement size
    int p = 1; // input size

    // declaration of system vectors/matrices
        vector<double> x(n), w(n), v(m), y(m), u(p);
        matrix<double> P(n,n);

    // Initial state
    x = zero_vector<double>(n);
    P = identity_matrix<double>(n)*0;
    y = zero_vector<double>(m);
    u = zero_vector<double>(p);

    // Kalman Filter
        vector<double> euler, eulerMag, dEuler;
    AhrsErrorDynamics md(&x, &u, &ins);
        md.initialize();
    KalmanFilter kf(&md, &y, &P);
    md.print();
    kf.print();

    // osg
    osg::Vec3 center(0,0,0), eye(0,20,-10), up(0,0,-1);
    vector<double> pos(3);
    osgViewer::Viewer *viewer = new osgViewer::Viewer;
    viewer->setUpViewInWindow(0,0,300,300);


    osg::Group *root = new osg::Group;
    Frame *frame = new uvsim::Frame(5);
    osg::PositionAttitudeTransform *frameTrans = new osg::PositionAttitudeTransform;
    frameTrans->addChild(frame->root);
    root->addChild(frameTrans);

    viewer->setSceneData(root);
    viewer->realize();

    // loop
    for (int i=0; !viewer->done(); i++)
    {

        if ((i % int(insFreq/kfFreq)) == 0)
        {
                        // measure error
                        eulerMag = measure(&ins);
                        euler = quat2Euler(ins.quat);
                        dEuler = euler - eulerMag;
                        y(0) = dEuler(2);
                        y(1) = dEuler(1);
                        y(2) = dEuler(0);

                        // update kalman filter
                        x = zero_vector<double>(n);
                        md.update();
                        md.discretize(kfFreq,3);
                        kf.update();
                        
                        // update ins
                        vector<double> deltaEuler(3);
                        deltaEuler(0) = x(2);
                        deltaEuler(1) = x(1);
                        deltaEuler(2) = x(0);   

                        ins.quat = euler2Quat(quat2Euler(ins.quat) - deltaEuler);       
                        //ins.quat = euler2Quat(eulerMag);
                        ins.gyroM = ins.gyroM + subrange(x,6,9);
                        ins.gyroB = ins.gyroB + subrange(x,3,6); 
        }

        if ((i % int(insFreq/dispFreq)) == 0)
        {
                    // update visual
            vector<double> quat = ins.getQuat();
            frameTrans->setAttitude(osg::Quat(quat(1),quat(2),quat(3),quat(0)));
            pos = ins.getGeo().getCartesian();
            //center.set(pos(0),pos(1),pos(2));

            //frameTrans->setPosition(osg::Vec3(pos(0), pos(1), pos(2)));
            viewer->getCamera()->setViewMatrixAsLookAt(eye, center, up);
            viewer->frame();

                // display to terminal
            clear();
            ins.print();
                        //md.print();
                        kf.print();
                        std::cout << "euler (deg) : " << euler*180/M_PI << std::endl;
                        std::cout << "eulerMag (deg) : " << eulerMag*180/M_PI << std::endl;
                        std::cout << "y (deg) : " << y*180/M_PI << std::endl;
                }

        // update
        ins.update();
                // hold position and velocity
                ins.velocity = zero_vector<double>(3);
                ins.longitude = ins.longitudeInit;
                ins.latitude = ins.latitudeInit;
                ins.altitude = ins.altitudeInit;
    }
    ins.stop();
    return(0);
}

vector<double> measure(Ins * ins)
{

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

        // unit vectors
        unit_z(0) = 0;
        unit_z(1) = 0;
        unit_z(2) = 1;
        unit_x(0) = 1;
        unit_x(1) = 0;
        unit_x(2) = 0;

        // rotation 0, alignment with gravity vector
        axis0 = norm(crossProd(-ins->accel, unit_z));
        angle0 = acos(inner_prod(-norm(ins->accel), unit_z));
        quat0 = axisAngle2Quat(axis0, angle0);

        // rotation 1, alignment with magnetic field
        magTempFrame = quatRotate(quat0, ins->mag);
        magTempFrame(2) = 0; // project to the NE frame
        magTempFrame = norm(magTempFrame);
        axis1 = norm(crossProd(magTempFrame, unit_x));
        angle1 = acos(inner_prod(unit_x, magTempFrame)) + M_PI/180*ins->geoMag->dec;
        quat1 = axisAngle2Quat(axis1, angle1);

        // initialize quaternions
        quatMag = quatProd(quat1,quat0);
        quatMag = norm(quatMag);
        return quat2Euler(quatMag);
}

// vim:ts=4:sw=4

---