/**************************************************************************
Copyright 1997 MBARI
***************************************************************************
Summary  : pan/tilt input filter
Filename : uFilter.h
Author   : Michael B. Matthews
Project  : New ROV
Version  : 1.0
Created  : 1.3.97
Modified : 11.9.97 - changed to 6th-order system
Notes    : This header file is automatically generated by MATLAB
         : routine uFilter.m, which uses a linear inertial model to
         : generate the filter system matrices F,D,H, and the Kalman
         : gain matrix K.
         : Filter order               - 6
         : Sample frequency           - 20 Hz
         : System time constant (tau) - 0.100000 Hz
         : Measurement noise variance - 0.000004
         : System noise variance      - 0.000000
***************************************************************************/

#ifndef uFilter_h
#define uFilter_h

#define U_SYSTEM_ORDER 6
#define U_INPUT_DIMENSION 1
#define U_OUTPUT_DIMENSION 1


// State transition matrix F
static Flt32 F[6][6] = {
{  1.0000000000,   0.0500000000,   0.0000000000,   0.0000000000,   0.0000000000,   0.0000000000 },
{  0.0000000000,   0.8818702526,   0.0000335372,   0.0000042519,  -0.0000071943,   0.0000120569 },
{  0.0000000000,  -0.0000000023,   0.8826172995,   0.2820043724,   0.0000000000,   0.0000000000 },
{  0.0000000000,  -0.0000000034,  -0.2820043724,   0.8826172995,   0.0000000000,   0.0000000000 },
{  0.0000000000,  -0.0000000048,   0.0000000000,   0.0000000000,   0.8841275592,   0.1321463164 },
{  0.0000000000,  -0.0000000020,   0.0000000000,   0.0000000000,  -0.1321463164,   0.8841275592 }
};


// Input matrix D
static Flt32 D[6][1] = {
{  0.0000000000 },
{  0.0015051125 },
{-388.1537817746 },
{-64.3698288794 },
{-1657.1221066450 },
{331.4617837647 }
};


// Output matrix H
static Flt32 H[1][6] = {
{  1.0000000000,   0.0000000000,   0.0000000000,   0.0000000000,   0.0000000000,   0.0000000000 }
};


// Kalman gain matrix K
static Flt32 K[6][1] = {
{  0.2462446283 },
{  0.0035597353 },
{ -0.0000000001 },
{  0.0000000000 },
{ -0.0000000001 },
{  0.0000000000 }
};



// Joystick model state transition matrix A
static Flt32 A[2][2] = {
{  1.0000000000,   0.0099326205 },
{  0.0000000000,   0.0067379470 }
};


// Joystick model input vector B
static Flt32 B[2] = {
 0.0400673795,
 0.9932620530
};



// Integrator a vector
static Flt32 aInt[2] = {
 1.0000000000,
-0.9500000000
};


// Integrator b vector
static Flt32 bInt[2] = {
 0.0250000000,
 0.0250000000
};

#endif