/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/dynamics/AhrsErrorDynamics.cc

Go to the documentation of this file.

/*
 * AhrsErrorDynamics.cc
 * Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
 *
 * AhrsErrorDynamics.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.
 *
 * AhrsErrorDynamics.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 "AhrsErrorDynamics.h"
#include "uvsim/utilities/utilities.h"
#include "uvsim/utilities/constants.h"

using namespace boost::numeric::ublas;

namespace uvsim
{

AhrsErrorDynamics::AhrsErrorDynamics(vector<double> *x, vector<double> *u, Ins *ins) :
        LinearModel(x,u), ins(ins) 
{
}

AhrsErrorDynamics::~AhrsErrorDynamics()
{
}

void AhrsErrorDynamics::initialize()
{
    size(9,3,1);
    update();

    // Ec, Process noise matrix
    Ec = identity_matrix<double>(n)*1;

    // L, Measurement noise matrix
    L = identity_matrix<double>(m)*1;

}


void AhrsErrorDynamics::update()
{
    matrix<double> C_bl(3,3), C_lb(3,3);
    double th, phi, gam;
        vector<double> w_il(3); 
        w_il(0) = ins->velocity(1)/(R0+ins->altitude) + omega*cos(ins->latitude);
        w_il(1) = -ins->velocity(0)/(R0+ins->altitude);
        w_il(2) = -ins->velocity(1)*tan(ins->latitude)/(R0+ins->altitude) - omega*sin(ins->latitude);


    phi = ins->euler(2);
    th = ins->euler(1);
    gam = ins->euler(0);

    // DCM from local tangent to body frame
    C_bl(0,0) = cos(th)*cos(phi);
    C_bl(1,0) = -cos(gam)*sin(phi) + sin(gam)*sin(th)*cos(phi);
    C_bl(2,0) = sin(gam)*sin(phi) + cos(gam)*sin(th)*cos(phi);
    C_bl(0,1) = cos(th)*sin(phi);
    C_bl(1,1) = cos(gam)*cos(phi) + sin(gam)*sin(th)*sin(phi);
    C_bl(2,1) = -sin(gam)*cos(phi) + cos(gam)*sin(th)*sin(phi);
    C_bl(0,2) = -sin(th);
    C_bl(1,2) = sin(gam)*cos(th);
    C_bl(2,2) = cos(gam)*cos(th);

    C_lb = trans(C_bl);

    // Ac, System matrix

        subrange(Ac,0,3,0,3) = skew(w_il);
        subrange(Ac,0,3,3,6) = -C_lb;
        subrange(Ac,0,3,6,7) = subrange(C_lb,0,3,0,1)*ins->gyro(0);
        subrange(Ac,0,3,7,8) = subrange(C_lb,0,3,1,2)*ins->gyro(1);
        subrange(Ac,0,3,8,9) = subrange(C_lb,0,3,2,3)*ins->gyro(2);

    // Bc, Input matrix

    // C, Measurement matrix
        //subrange(C,0,3,0,3) = C_bl;
        C(0,0) = -tan(th)*cos(phi);
        C(0,1) = -tan(th)*sin(phi);
        C(0,2) = -1;
        C(1,0) = sin(th);
        C(1,1) = -cos(th);
        C(2,0) = -cos(phi)/cos(th);
        C(2,1) = -sin(phi)/cos(th);
}

} // uvsim


// vim:ts=4:sw=4

---