From a44e33bdafdf1300facb2147f8a1634aa9ec19c5 Mon Sep 17 00:00:00 2001
From: Tim Trueman <tim.trueman@gmail.com>
Date: Fri, 30 Dec 2011 08:54:45 -0800
Subject: [PATCH] Working on getting cpress working

---
 .gitignore   |   3 +
 autopilot.py |  59 ++++
 dcm.py       | 802 +++++++++++++++++++++++++++++++++++++++++++++++++++
 ekf.py       |  85 ++++--
 fgo.py       |   4 +-
 shumai.py    |  49 ++--
 vgo.py       |   2 +-
 7 files changed, 956 insertions(+), 48 deletions(-)
 create mode 100644 .gitignore
 create mode 100644 autopilot.py
 create mode 100644 dcm.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..c06bc88
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+*.log
+*.csv
+*.sql
diff --git a/autopilot.py b/autopilot.py
new file mode 100644
index 0000000..5bb3b52
--- /dev/null
+++ b/autopilot.py
@@ -0,0 +1,59 @@
+from truthdata import TruthData
+from math import degrees, radians, atan
+from display import Display
+from utils import GRAVITY
+
+TD = TruthData()
+display = Display()
+
+class Autopilot:
+
+    def __init__(self):
+        self.pitch_cmd = 10.0
+        self.roll_cmd = 0.0
+        self.hdg_cmd = 0.0
+        self.xplane_gain = 1 / 45.0
+
+    def condition_heading(self, heading):
+        heading_error = self.hdg_cmd - heading
+        if heading > 180: # never goes <180 from GPS
+            heading = heading - 360
+            heading_error = self.hdg_cmd - heading
+        if self.hdg_cmd - heading > 180:
+            heading_error = (self.hdg_cmd - heading) - 360;
+        if self.hdg_cmd - heading < -180:
+            heading_error = (self.hdg_cmd - heading) + 360;
+        # display.register_scalars({"heading_error": heading_error,})
+        heading_error = min(30, max(-30, heading_error))
+        return heading_error
+
+    def heading_hold(self):
+        kp_psi = 0.75
+        trim_airspeed = 45.0
+        kp_deweight = 0.1
+        hdg_deweight = (trim_airspeed / TD.SPEEDOVERGROUND) * kp_deweight
+        hdg_deweight = min(1.6, max(0.6, hdg_deweight))
+        kp_psi_dot = 0.333
+        psi_dot_cmd = self.condition_heading(degrees(TD.HEADING)) * kp_psi_dot
+        psi_dot_cmd = min(6, max(-6, psi_dot_cmd))
+        psi_dot_cmd = radians(psi_dot_cmd)
+        self.roll_cmd = atan(psi_dot_cmd * (TD.SPEEDOVERGROUND * 1.68780986) / 32.2)
+        self.roll_cmd = min(30, max(-30, degrees(self.roll_cmd)))
+        # display.register_scalars({"roll_cmd": self.roll_cmd,"psi_dot_cmd": degrees(psi_dot_cmd),"hdg_deweight": hdg_deweight}, "Autopilot debugging")
+        # display.register_scalars({"roll_cmd": self.roll_cmd,"psi_dot_cmd": degrees(psi_dot_cmd),})
+        # self.roll_cmd = kp_psi * hdg_deweight * self.condition_heading(degrees(TD.HEADING))
+        return self.roll_cmd
+
+    def pitch_hold(self):
+        kp_theta = 0.0
+        self.ele_cmd = kp_theta * (self.pitch_cmd - degrees(TD.PITCH)) * self.xplane_gain
+        return self.ele_cmd
+
+    def roll_hold(self):
+        kp_phi = 1.0
+        self.ail_cmd = kp_phi * (self.roll_cmd - degrees(TD.ROLL)) * self.xplane_gain
+        #display.register_scalars({"ail_cmd": self.ail_cmd,"wtf": self.roll_cmd - degrees(TD.ROLL),"roll_cmd":self.roll_cmd, "td_roll":degrees(TD.ROLL)}, "Autopilot debugging")
+        return self.ail_cmd
+
+    def throttle(self):
+        return 0.4
diff --git a/dcm.py b/dcm.py
new file mode 100644
index 0000000..86de18c
--- /dev/null
+++ b/dcm.py
@@ -0,0 +1,802 @@
+from math import *
+
+'''
+Alternate algorithms (VGO, DCM, EKF Euler angles versus quaternions) could provide a gut check / fall-back option as a fail-safe...
+
+This didn't get very far.
+
+DCM - Bill Premerlani & Paul Bizard / DIY Drones
+Inputs:
+    gyros (rad/s)
+    accelerometers (m/s^2)
+    GPS vx vy horizontal speed (m/s)
+    GPS course over ground (rad)
+    GPS speeed Ve (m/s)
+Outputs
+    y0[0 1 2] euler angels (rad)
+    y1[0 1 2]
+    y2[0 1 2]
+    y3[0 1 2]
+'''
+
+class DCM:
+
+    def __init__(self):
+        self.psi = 0.0
+        self.theta = 0.023865
+        self.phi = 0.0
+        psi = self.psi
+        theta = self.theta
+        phi = self.phi
+        self.R[0,0] = cos(theta) * cos(psi)
+        self.R[0,1] = cos(theta) * sin(psi)
+        self.R[0,2] = -sin(theta)
+        self.R[1,0] = (sin(phi) * sin(theta) * cos(psi)) - (cos(phi) * sin(psi))
+        self.R[1,1] = (sin(phi) *sin(theta) * sin(psi)) + (cos(phi) * cos(psi))
+        self.R[1,2] = sin(phi) * cos(theta)
+        self.R[2,0] = (cos(phi) * sin(theta) * cos(psi)) + (sin(phi) * sin(psi))
+        self.R[2,1] = (cos(phi) * sin(theta) * sin(psi)) - (sin(phi) * cos(psi))
+        self.R[2,2] = cos(phi) * cos(theta)
+        self.rup = matrix("1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0")
+        self.integral = matrix("0.0 0.0 0.0")
+        self.Rfilter0 = R[0,0]
+        self.Rfilter3 = R[1,0]
+        self.yaw_flag = 0.0
+        self.GPSspeed = matrix("26.66 26.66")
+        self.omega = matrix("0.0 0.0 0.0")
+
+    def model_outputs(self):
+        self.psi = atan2(self.R[1,0],self.R[0,0])
+        self.theta = -asin(self.R[2,0])
+        self.phi = atan2(self.R[2,1],self.R[2,2])
+        return {"psi": self.psi,
+                "theta": self.theta,
+                "phi": self.phi,}
+
+    def model_update(self, dt):
+        omega_correction = matrix("%f %f %f" % omega_correction_p_acc, omega_correction_p_gps, omega_correction_integral)
+        self.omega += omega_correction
+        self.theta_hat = omega * dt
+        self.rup[0,0]
+        
+
+# /*============================================================================*/
+# /* SFUNCTION NUMERICAL CONTROLLER                                             */
+# /*============================================================================*/
+# /* Authors: Bill PREMERLANI & Paul BIZARD / DIY Drones                        */
+# /* Date   : February 2009                                                     */
+# /*============================================================================*/
+# /* INPUTS                                                                     */
+# /* ------                                                                     */
+# /* u0[0 1 2] Gyrometers p q r             rad/s                               */
+# /* u1[0 1 2] Accelerometers x y z         m/s^2                               */
+# /* u2[0 1]   GPS vx vy horizontal speed   m/s                                 */
+# /* u3[0]     GPS Khi course over ground   rad                                 */
+# /* u4[0]     GPS speed Ve                 m/s                                 */
+# /*============================================================================*/
+# /* OUTPUTS                                                                    */
+# /* -------                                                                    */
+# /* y0[0 1 2] Euler angles           rad                                       */
+# /* y1[0 1 2]                                                                  */
+# /* y2[0 1 2]                                                                  */
+# /* y3[0 1 2]                                                                  */
+# /*============================================================================*/
+# /* DCM matrix estimation                                                      */
+# /* R marix = transformation from body frame to earth frame                    */
+# /*                                                                            */
+# /* 1st column is x body vector expressed in earth frame                       */
+# /* 2nd...        y                                                            */
+# /* 3rd...        z                                                            */
+# /*                                                                            */
+# /* 1st row is x earth vector expressed in body frame                          */
+# /* 2nd row    y                                                               */
+# /* 3rd row    z                                                               */
+# /*                                                                            */
+# /* x front                                                                    */
+# /* y right wing                                                               */
+# /* z down                                                                     */
+# /*============================================================================*/
+
+# #define S_FUNCTION_NAME       R
+# #define S_FUNCTION_LEVEL      2
+# #define NUM_INPUTS            5
+# #define INPUT_0_WIDTH         3
+# #define INPUT_1_WIDTH         3
+# #define INPUT_2_WIDTH         2
+# #define INPUT_3_WIDTH         1
+# #define INPUT_4_WIDTH         1
+# #define INPUT_0_FEEDTHROUGH   1
+# #define INPUT_1_FEEDTHROUGH   1
+# #define INPUT_2_FEEDTHROUGH   1
+# #define INPUT_3_FEEDTHROUGH   1
+# #define INPUT_4_FEEDTHROUGH   1
+# #define NUM_OUTPUTS           6
+# #define OUTPUT_0_WIDTH        3
+# #define OUTPUT_1_WIDTH        3
+# #define OUTPUT_2_WIDTH        3
+# #define OUTPUT_3_WIDTH        3
+# #define OUTPUT_4_WIDTH        3
+# #define OUTPUT_5_WIDTH        3
+# #define NPARAMS               0
+# #define SAMPLE_TIME_0         0.025
+# #define SAMPLE_TIME_1         1.  
+# #define NUM_DISC_STATES       0
+# #define DISC_STATES_IC        [0]
+# #define NUM_CONT_STATES       0
+# #define CONT_STATES_IC        [0]
+# #define SFUNWIZ_GENERATE_TLC  1
+# #define SOURCEFILES           ""
+# #define PANELINDEX            5
+# #define SFUNWIZ_REVISION      1.0
+# 
+# #define DT25          0.025     // 25ms sample time
+# #define GPSFILT       0.012422  // x body filter coefficient = 1-exp(-DT25/2s)
+# 
+# #define KPPitchRoll   0.012   // Pitch-Roll correction proportional gain
+# #define KIPitchRoll   0.00002 // Pitch-Roll correction integral gain
+# #define KPYaw         0.3 // Yaw correction proportional gain
+# #define KIYaw         0.0001  // Yaw correction integral gain
+# 
+# #define R2D           57.295779 // Radians to degrees
+# #define D2R           0.0174532 // Degrees to radians
+# 
+# #define PARAM_DEF0(S) ssGetSFcnParam(S, 0)
+# 
+# #define MDL_CHECK_PARAMETERS
+# #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
+# /*----------------------------------------------------------------------------*/
+# static void mdlCheckParameters(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+#    unsigned short int  i;
+#    bool                validParam = false;
+# 
+# 
+#    /* All parameters must be scalar */
+#    for (i=0;i<ssGetSFcnParamsCount(S);i++)
+#    {
+#       const mxArray *pVal = ssGetSFcnParam(S,i);
+# 
+#       if (!mxIsNumeric(pVal) || !mxIsDouble(pVal) ||  mxIsLogical(pVal)
+#        ||  mxIsComplex(pVal) ||  mxIsSparse(pVal) 
+#        || !mxIsFinite(mxGetPr(pVal)[0]))
+#       {
+#          validParam = true;
+#          break;
+#       }
+#    }
+# 
+#    if (validParam) 
+#    {
+#       ssSetErrorStatus(S,"All parameters must be a scalar or vectors");     
+#       return;
+#    }
+# 
+#    /* All parameters are not tunable */
+#    for (i=0;i<ssGetSFcnParamsCount(S);i++)
+#    {
+#       ssSetSFcnParamTunable(S, i, 0);
+#    }
+# }
+# #endif
+# 
+# /*----------------------------------------------------------------------------*/
+# static void mdlInitializeSizes(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+#    ssSetNumSFcnParams(S, NPARAMS);           /* Number of expected parameters */
+# #if defined(MATLAB_MEX_FILE)
+#    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S))
+#    {
+#       mdlCheckParameters(S);
+#       if (ssGetErrorStatus(S) != NULL) return;
+#    }
+#    /* Parameter mismatch will be reported by Simulink */
+#    else  return;
+# #endif
+#     
+#    ssSetNumContStates(S, NUM_CONT_STATES);
+#    ssSetNumDiscStates(S, NUM_DISC_STATES);
+# 
+#    if (!ssSetNumInputPorts(S, NUM_INPUTS)) return;
+# 
+#    ssSetInputPortWidth(S, 0, INPUT_0_WIDTH);
+#    ssSetInputPortDirectFeedThrough(S, 0, INPUT_0_FEEDTHROUGH);
+#    ssSetInputPortRequiredContiguous(S, 0, 1);
+# 
+#    ssSetInputPortWidth(S, 1, INPUT_1_WIDTH);
+#    ssSetInputPortDirectFeedThrough(S, 1, INPUT_1_FEEDTHROUGH);
+#    ssSetInputPortRequiredContiguous(S, 1, 1);
+# 
+#    ssSetInputPortWidth(S, 2, INPUT_2_WIDTH);
+#    ssSetInputPortDirectFeedThrough(S, 2, INPUT_2_FEEDTHROUGH);
+#    ssSetInputPortRequiredContiguous(S, 2, 1);
+# 
+#    ssSetInputPortWidth(S, 3, INPUT_3_WIDTH);
+#    ssSetInputPortDirectFeedThrough(S, 3, INPUT_3_FEEDTHROUGH);
+#    ssSetInputPortRequiredContiguous(S, 3, 1);
+# 
+#    ssSetInputPortWidth(S, 4, INPUT_4_WIDTH);
+#    ssSetInputPortDirectFeedThrough(S, 4, INPUT_4_FEEDTHROUGH);
+#    ssSetInputPortRequiredContiguous(S, 4, 1);
+# 
+#    if (!ssSetNumOutputPorts(S, NUM_OUTPUTS)) return;
+#    ssSetOutputPortWidth(S, 0, OUTPUT_0_WIDTH);
+#    ssSetOutputPortWidth(S, 1, OUTPUT_1_WIDTH);
+#    ssSetOutputPortWidth(S, 2, OUTPUT_2_WIDTH);
+#    ssSetOutputPortWidth(S, 3, OUTPUT_3_WIDTH);
+#    ssSetOutputPortWidth(S, 4, OUTPUT_4_WIDTH);
+#    ssSetOutputPortWidth(S, 5, OUTPUT_5_WIDTH);
+# 
+#    ssSetNumSampleTimes(S, 2);
+#    ssSetNumRWork(S, 0);
+#    ssSetNumIWork(S, 0);
+#    ssSetNumPWork(S, 17);
+#    ssSetNumModes(S, 0);
+#    ssSetNumNonsampledZCs(S, 0);
+# 
+#    /* Take care when specifying exception free code - see sfuntmpl_doc.c */
+#    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE
+#                  | SS_OPTION_DISCRETE_VALUED_OUTPUT);
+# }
+# 
+# /*----------------------------------------------------------------------------*/
+# static void mdlInitializeSampleTimes(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+#    ssSetSampleTime(S, 0, SAMPLE_TIME_0);
+#    ssSetOffsetTime(S, 0, 0.0);
+#    ssSetSampleTime(S, 1, SAMPLE_TIME_1);
+#    ssSetOffsetTime(S, 1, 0.0);
+# }
+# 
+# #define MDL_START
+# #if defined(MDL_START)
+# /*----------------------------------------------------------------------------*/
+# static void mdlStart(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+#    /* Declarations */
+#    /* ============ */
+#    float  *rmat;
+#    float  *rup;
+#    float  *errorRP;
+#    float  *omegacorrPAcc;
+#    float  *omegacorrInt;
+#    float  *Integral;
+#    float  *rmat0filt;
+#    float  *rmat3filt;
+#    float  *dirovergndHRmat;
+#    float  *dirovergndHGPS;
+#    float  *errorYawground;
+#    float  *errorYawplane;
+#    float  *omegacorrPGPS;
+#    float  *sogGPS;
+#    float  *speedGPS;
+#    float  *accelGPS;
+#    int    *flags;
+# 
+# 
+#    /* Allocate memory to arrays */
+#    /* ========================= */
+#    rmat            = (float *) calloc(9, sizeof(float));
+#    rup             = (float *) calloc(9, sizeof(float));
+#    errorRP         = (float *) calloc(3, sizeof(float));
+#    omegacorrPAcc   = (float *) calloc(3, sizeof(float));
+#    omegacorrInt    = (float *) calloc(3, sizeof(float));
+#    Integral        = (float *) calloc(3, sizeof(float));
+#    rmat0filt       = (float *) calloc(1, sizeof(float));
+#    rmat3filt       = (float *) calloc(1, sizeof(float));
+#    dirovergndHRmat = (float *) calloc(3, sizeof(float));
+#    dirovergndHGPS  = (float *) calloc(3, sizeof(float));
+#    errorYawground  = (float *) calloc(3, sizeof(float));
+#    errorYawplane   = (float *) calloc(3, sizeof(float));
+#    omegacorrPGPS   = (float *) calloc(3, sizeof(float));
+#    sogGPS          = (float *) calloc(1, sizeof(float));
+#    speedGPS        = (float *) calloc(2, sizeof(float));
+#    accelGPS        = (float *) calloc(1, sizeof(float));
+#    flags           = (int *)   calloc(1, sizeof(int));
+# 
+# 
+#    /* Attach pointers as user data */
+#    /* ============================ */
+#    ssGetPWork(S)[0]  = rmat;
+#    ssGetPWork(S)[1]  = rup;
+#    ssGetPWork(S)[2]  = errorRP;
+#    ssGetPWork(S)[3]  = omegacorrPAcc;
+#    ssGetPWork(S)[4]  = omegacorrInt;
+#    ssGetPWork(S)[5]  = Integral;
+#    ssGetPWork(S)[6]  = rmat0filt;
+#    ssGetPWork(S)[7]  = rmat3filt;
+#    ssGetPWork(S)[8]  = dirovergndHRmat;
+#    ssGetPWork(S)[9]  = dirovergndHGPS;
+#    ssGetPWork(S)[10] = errorYawground;
+#    ssGetPWork(S)[11] = errorYawplane;
+#    ssGetPWork(S)[12] = omegacorrPGPS;
+#    ssGetPWork(S)[13] = sogGPS;
+#    ssGetPWork(S)[14] = speedGPS;
+#    ssGetPWork(S)[15] = accelGPS;
+#    ssGetPWork(S)[16] = flags;
+# }
+# #endif
+# 
+# #define MDL_INITIALIZE_CONDITIONS
+# #if defined(MDL_INITIALIZE_CONDITIONS)
+# /*----------------------------------------------------------------------------*/
+# static void mdlInitializeConditions(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+#    /* Declarations */
+#    /* ============ */
+#    float  *rmat      = ssGetPWork(S)[0];
+#    float  *rup       = ssGetPWork(S)[1];
+#    float  *Integral  = ssGetPWork(S)[5];
+#    float  *rmat0filt = ssGetPWork(S)[6];
+#    float  *rmat3filt = ssGetPWork(S)[7];
+#    float  *speedGPS  = ssGetPWork(S)[14];
+#    int    *flags     = ssGetPWork(S)[16];
+# 
+#    float  Yaw, Pitch, Roll;
+#    int    i;
+# 
+# 
+#    /* Initial Euler angles */
+#    /* ==================== */
+#    Yaw   = 0. * D2R;
+#    Pitch = 0.023865;
+#    Roll  = 0. * D2R;
+# 
+# 
+#    /* Initialize DCM */
+#    /* ============== */
+#    rmat[0] =  cos(Yaw)*cos(Pitch);
+#    rmat[1] = -sin(Yaw)*cos(Roll) + cos(Yaw)*sin(Pitch)*sin(Roll);
+#    rmat[2] =  sin(Yaw)*sin(Roll) + cos(Yaw)*sin(Pitch)*cos(Roll);
+#    rmat[3] =  sin(Yaw)*cos(Pitch);
+#    rmat[4] =  cos(Yaw)*cos(Roll) + sin(Yaw)*sin(Pitch)*sin(Roll);
+#    rmat[5] = -cos(Yaw)*sin(Roll) + sin(Yaw)*sin(Pitch)*cos(Roll);
+#    rmat[6] = -sin(Pitch);
+#    rmat[7] =  cos(Pitch)*sin(Roll);
+#    rmat[8] =  cos(Pitch)*cos(Roll);
+# 
+# 
+#    /* Update matrix rup = identity */
+#    /* ============================ */
+#    for (i=0;i<9;i++) rup[i] = 0.;
+#    rup[0] = 1.; rup[4] = 1.; rup[8] = 1.;
+# 
+# 
+#    /* Integrator state */
+#    /* ================ */
+#    for (i=0;i<3;i++) Integral[i] = 0.;
+# 
+# 
+#    /* X body over ground filter */
+#    /* ========================= */
+#    rmat0filt[0] = rmat[0];
+#    rmat3filt[0] = rmat[3];
+# 
+# 
+#    /* Yaw flag */
+#    /* ======== */
+#    flags[0] = 0;
+# 
+# 
+#    /* GPS speed */
+#    /* ========= */
+#    speedGPS[0] = 26.66;
+#    speedGPS[1] = 26.66;
+# }
+# #endif
+# 
+# #define MDL_OUTPUTS
+# #if defined(MDL_OUTPUTS)
+# /*----------------------------------------------------------------------------*/
+# static void mdlOutputs(SimStruct *S, int_T tid)
+# /*----------------------------------------------------------------------------*/
+# {
+#    real_T *y0 = ssGetOutputPortRealSignal(S,0);
+#    real_T *y1 = ssGetOutputPortRealSignal(S,1);
+#    real_T *y2 = ssGetOutputPortRealSignal(S,2);
+#    real_T *y3 = ssGetOutputPortRealSignal(S,3);
+#    real_T *y4 = ssGetOutputPortRealSignal(S,4);
+#    real_T *y5 = ssGetOutputPortRealSignal(S,5);
+# 
+#    float  *rmat          = ssGetPWork(S)[0];
+#    float  *omegacorrPAcc = ssGetPWork(S)[3];
+#    float  *omegacorrInt  = ssGetPWork(S)[4];
+#    float  *omegacorrPGPS = ssGetPWork(S)[12];
+#    float  *speedGPS      = ssGetPWork(S)[14];
+#    float  *accelGPS      = ssGetPWork(S)[15];
+# 
+#    float  Yaw, Pitch, Roll;
+# 
+# 
+#    /* Euler angles from DCM */
+#    /* ===================== */
+#    /* atan2(rmat31,rmat11) */
+#    Yaw = atan2(rmat[3],rmat[0]);
+# 
+#    /* -asin(rmat31) */
+#    Pitch = -asin(rmat[6]);   
+# 
+#    /* atan2(rmat32,rmat33) */
+#    Roll = atan2(rmat[7],rmat[8]);
+# 
+# 
+#    /* Outputs */
+#    /* ======= */
+#    /* Euler angles */
+#    y0[0] = Roll; 
+#    y0[1] = Pitch; 
+#    y0[2] = Yaw; 
+# 
+#    /* Roll-Pitch correction proportional term */
+#    y1[0] = omegacorrPAcc[0]; 
+#    y1[1] = omegacorrPAcc[1];
+#    y1[2] = omegacorrPAcc[2];
+# 
+#    /* Yaw correction proportional term */
+#    y2[0] = omegacorrPGPS[0]; 
+#    y2[1] = omegacorrPGPS[1]; 
+#    y2[2] = omegacorrPGPS[2]; 
+# 
+#    /* Roll-Pitch + Yaw integral term */
+#    y3[0] = omegacorrInt[0]; 
+#    y3[1] = omegacorrInt[1]; 
+#    y3[2] = omegacorrInt[2]; 
+# 
+#    /* Debug 1 */
+#    y4[0] = 0.; 
+#    y4[1] = 0.;
+#    y4[2] = 0.;
+# 
+#    /* Debug 2 */
+#    y5[0] = 0.; 
+#    y5[1] = 0.;
+#    y5[2] = 0.;
+# }
+# #endif
+# 
+# #define MDL_UPDATE
+# #if defined(MDL_UPDATE)
+# /*----------------------------------------------------------------------------*/
+# static void mdlUpdate(SimStruct *S, int_T tid)
+# /*----------------------------------------------------------------------------*/
+# {
+#    /* Declarations */
+#    /* ============ */
+#    real_T *u0 = (const real_T *) ssGetInputPortSignal(S,0);
+#    real_T *u1 = (const real_T *) ssGetInputPortSignal(S,1);
+#    real_T *u2 = (const real_T *) ssGetInputPortSignal(S,2);
+#    real_T *u3 = (const real_T *) ssGetInputPortSignal(S,3);
+#    real_T *u4 = (const real_T *) ssGetInputPortSignal(S,4);
+# 
+#    float  *rmat            = ssGetPWork(S)[0];
+#    float  *rup             = ssGetPWork(S)[1];
+#    float  *errorRP         = ssGetPWork(S)[2];
+#    float  *omegacorrPAcc   = ssGetPWork(S)[3];
+#    float  *omegacorrInt    = ssGetPWork(S)[4];
+#    float  *Integral        = ssGetPWork(S)[5];
+#    float  *rmat0filt       = ssGetPWork(S)[6];
+#    float  *rmat3filt       = ssGetPWork(S)[7];
+#    float  *dirovergndHRmat = ssGetPWork(S)[8];
+#    float  *dirovergndHGPS  = ssGetPWork(S)[9];
+#    float  *errorYawground  = ssGetPWork(S)[10];
+#    float  *errorYawplane   = ssGetPWork(S)[11];
+#    float  *omegacorrPGPS   = ssGetPWork(S)[12];
+#    float  *sogGPS          = ssGetPWork(S)[13];
+#    float  *speedGPS        = ssGetPWork(S)[14];
+#    float  *accelGPS        = ssGetPWork(S)[15];
+#    int    *flags           = ssGetPWork(S)[16];
+# 
+#    float  omegatotal[3], theta[3];
+#    float  V_buff[3];
+#    float  r_buff[9];
+#    float  f_buff;
+# 
+#    int    i;
+# 
+#    FILE*  deb_file_ptr;
+# 
+# 
+# /*----------------------------------------------------------------------------*/
+# //   /* Open debug file */
+# //   deb_file_ptr = fopen ("deb_file.txt", "a");
+# /*----------------------------------------------------------------------------*/
+# 
+# /*============================================================================*/
+# /*============================================================================*/
+# /*                                25ms routine                                */
+# /*============================================================================*/
+# /*============================================================================*/
+# 
+#    /* If 25ms sample time */
+#    if (ssIsSampleHit(S,0,tid))
+#    {
+#       /* R matrix update */
+#       /* =============== */
+#       /* =============== */
+#       /* Read body angular velocity (p q r gyrometers) */
+#       for (i=0;i<3;i++) omegatotal[i] = u0[i];
+# 
+#       /* Add pitch-roll and yaw corrections */
+#       VectorAdd (omegatotal, omegacorrPAcc, omegatotal);
+#       VectorAdd (omegatotal, omegacorrPGPS, omegatotal);
+#       VectorAdd (omegatotal, omegacorrInt,  omegatotal);
+# 
+#       /* Integrate angular velocity over the 25ms time step */
+#       for (i=0;i<3;i++) theta[i] = omegatotal[i] * DT25;
+# 
+#       /* Assemble equivalent small rotation matrix (update matrix) */
+#       rup[1] = -theta[2];
+#       rup[2] =  theta[1];
+#       rup[3] =  theta[2];
+#       rup[5] = -theta[0];
+#       rup[6] = -theta[1];
+#       rup[7] =  theta[0];
+# 
+#       /* Rotate body frame */
+#       MatrixMultiply (rmat, rup);
+# 
+# 
+#       /* Roll-Pitch correction */
+#       /* ===================== */
+#       /* ===================== */
+#       /* Read body acceleration (x y z accelerometers) */
+#       for (i=0;i<3;i++) V_buff[i] = u1[i];
+# 
+#       /* Correct body acceleration for centrifugal effect */
+#       V_buff[0] += accelGPS[0];      
+#       V_buff[1] += (u0[2]+omegacorrInt[2])*speedGPS[0];      
+#       V_buff[2] -= (u0[1]+omegacorrInt[1])*speedGPS[0];      
+# 
+#       /* Compute the roll-pitch error vector: cross product of measured */
+#       /* earth Z vector with estimated earth vector expressed in body   */
+#       /* frame (3rd row of rmat)                                        */
+#       VectorCross(V_buff, &rmat[6], errorRP);
+# 
+#       /* Compute pitch-roll correction proportional term */
+#       for (i=0;i<3;i++) omegacorrPAcc[i] = errorRP[i] * KPPitchRoll;
+# 
+#       /* Add pitch-roll error to integrator */
+#       for (i=0;i<3;i++) Integral[i] += errorRP[i] * KIPitchRoll;
+# 
+# 
+#       /* Yaw correction part 2 */
+#       /* ===================== */
+#       /* ===================== */
+#       /* Filter with a 1st order the x body vector over ground */ 
+#       rmat0filt[0] += GPSFILT * (rmat[0] - rmat0filt[0]);
+#       rmat3filt[0] += GPSFILT * (rmat[3] - rmat3filt[0]);
+# 
+# 
+#       /* If direction over ground has been updated */
+#       if (flags[0] == 1)
+#       {
+#          /* Compute the yaw error vector expressed in earth frame */
+#          VectorCross(dirovergndHRmat, dirovergndHGPS, errorYawground);
+# 
+#          /* Express the yaw error vector in the body frame */
+#          TransposeMatrix(rmat, r_buff);
+#          MatrixVector(r_buff, errorYawground, errorYawplane);
+# 
+#          /* Compute yaw correction proportional term */
+#          for (i=0;i<3;i++) omegacorrPGPS[i] = errorYawplane[i] * KPYaw;
+# 
+#          /* Reset yaw correction flag */
+#          flags[0] = 0;
+#       }
+# 
+#       /* Add yaw error to integrator */
+#       for (i=0;i<3;i++) Integral[i] += errorYawplane[i] * KIYaw;
+# 
+#       /* Compute integral term */
+#       for (i=0;i<3;i++) omegacorrInt[i] = Integral[i];
+#    }
+# 
+# /*============================================================================*/
+# /*============================================================================*/
+# /*                                1s routine                                  */
+# /*============================================================================*/
+# /*============================================================================*/
+# 
+#    /* If 1s sample time */
+#    if (ssIsSampleHit(S,1,tid))
+#    {
+#       /* Orthogonalization */
+#       /* ================= */
+#       /* ================= */
+#       /* (U,V) */
+#       VectorDotProduct(&rmat[0], &rmat[3], &f_buff);
+#       f_buff /= 2.;
+# 
+#       /* U = U - 0.5*V(U,V) */
+#       for (i=0;i<3;i++) V_buff[i] = rmat[i];
+#       for (i=0;i<3;i++) rmat[i]   -= rmat[3+i]*f_buff;
+# 
+#       /* V = V - 0.5*U(U,V) */
+#       for (i=0;i<3;i++) rmat[3+i] -= V_buff[i]*f_buff;
+# 
+#       /* W = UxV */
+#       VectorCross(&rmat[0], &rmat[3], &rmat[6]);
+# 
+#       /* U scaling */
+#       VectorDotProduct(&rmat[0], &rmat[0], &f_buff);
+#       f_buff = 1./sqrt(f_buff);
+#       for (i=0;i<3;i++) rmat[i] = rmat[i]*f_buff;
+# 
+#       /* V scaling */
+#       VectorDotProduct(&rmat[3], &rmat[3], &f_buff);
+#       f_buff = 1./sqrt(f_buff);
+#       for (i=0;i<3;i++) rmat[3+i] = rmat[3+i]*f_buff;
+# 
+#       /* W scaling */
+#       VectorDotProduct(&rmat[6], &rmat[6], &f_buff);
+#       f_buff = 1./sqrt(f_buff);
+#       for (i=0;i<3;i++) rmat[6+i] = rmat[6+i]*f_buff;
+# 
+# 
+#       /* Yaw correction part 1 */
+#       /* ===================== */
+#       /* ===================== */
+#       /* Filtered x body over ground (projection of x body on earth xy plane) */
+#       dirovergndHRmat[0] = rmat0filt[0];
+#       dirovergndHRmat[1] = rmat3filt[0];
+#       dirovergndHRmat[2] = 0.;
+# 
+#       /* Normalized GPS speed over ground vector */
+#       dirovergndHGPS[0] = cos(u3[0]);
+#       dirovergndHGPS[1] = sin(u3[0]);
+#       dirovergndHGPS[2] = 0.;
+# 
+#       /* Set yaw correction flag */
+#       flags[0] = 1;
+# 
+# 
+#       /* SOG from GPS */
+#       /* ============ */
+#       /* ============ */
+#       /* Read speed from GPS */
+#       speedGPS[1] = speedGPS[0];
+#       speedGPS[0] = u4[0];
+#       accelGPS[0] = speedGPS[0] - speedGPS[1];
+#    }
+# 
+# /*----------------------------------------------------------------------------*/
+# //   /* Close debug file */
+# //   fclose (deb_file_ptr);
+# /*----------------------------------------------------------------------------*/
+# } 
+# #endif
+# 
+# /*----------------------------------------------------------------------------*/
+# static void MatrixMultiply(float *M1, float *M2)
+# /*----------------------------------------------------------------------------*/
+# /* M1 = M1*M2                                                                 */
+# /*----------------------------------------------------------------------------*/
+# {
+#    /* Declarations */
+#    float  M_buff[9];
+#    int    i;
+# 
+# 
+#    for (i=0;i<9;i++) M_buff[i] = M1[i];
+# 
+#    M1[0] = M_buff[0]*M2[0] + M_buff[1]*M2[3] + M_buff[2]*M2[6];    
+#    M1[3] = M_buff[3]*M2[0] + M_buff[4]*M2[3] + M_buff[5]*M2[6];    
+#    M1[6] = M_buff[6]*M2[0] + M_buff[7]*M2[3] + M_buff[8]*M2[6];    
+# 
+#    M1[1] = M_buff[0]*M2[1] + M_buff[1]*M2[4] + M_buff[2]*M2[7];    
+#    M1[4] = M_buff[3]*M2[1] + M_buff[4]*M2[4] + M_buff[5]*M2[7];    
+#    M1[7] = M_buff[6]*M2[1] + M_buff[7]*M2[4] + M_buff[8]*M2[7];    
+# 
+#    M1[2] = M_buff[0]*M2[2] + M_buff[1]*M2[5] + M_buff[2]*M2[8];    
+#    M1[5] = M_buff[3]*M2[2] + M_buff[4]*M2[5] + M_buff[5]*M2[8];    
+#    M1[8] = M_buff[6]*M2[2] + M_buff[7]*M2[5] + M_buff[8]*M2[8];    
+# }
+# 
+# /*----------------------------------------------------------------------------*/
+# static void TransposeMatrix(float *M1, float *M2)
+# /*----------------------------------------------------------------------------*/
+# /* M2 = Transpose M1                                                          */
+# /*----------------------------------------------------------------------------*/
+# {
+#    M2[0] = M1[0]; M2[1] = M1[3]; M2[2] = M1[6];
+#    M2[3] = M1[1]; M2[4] = M1[4]; M2[5] = M1[7];
+#    M2[6] = M1[2]; M2[7] = M1[5]; M2[8] = M1[8];
+# }
+# 
+# /*----------------------------------------------------------------------------*/
+# static void MatrixVector(float *M, float *V1, float *V2)
+# /*----------------------------------------------------------------------------*/
+# /* V2 = M*V1                                                                  */
+# /*----------------------------------------------------------------------------*/
+# {
+#    V2[0] = M[0]*V1[0] + M[1]*V1[1] + M[2]*V1[2];
+#    V2[1] = M[3]*V1[0] + M[4]*V1[1] + M[5]*V1[2];
+#    V2[2] = M[6]*V1[0] + M[7]*V1[1] + M[8]*V1[2];
+# }
+# 
+# 
+# /*----------------------------------------------------------------------------*/
+# static void VectorDotProduct(float *V1, float *V2, float *Scalar_Product)
+# /*----------------------------------------------------------------------------*/
+# /* Scalar_Product = V1.V2                                                     */
+# /*----------------------------------------------------------------------------*/
+# {
+#    /* Declarations */
+#    int    i;
+# 
+# 
+#    *Scalar_Product = 0.;
+#    for (i=0;i<3;i++) *Scalar_Product += V1[i]*V2[i];
+# }
+# 
+# /*----------------------------------------------------------------------------*/
+# static void VectorCross(float *V1, float *V2, float *V3)
+# /*----------------------------------------------------------------------------*/
+# /* V3 = V1xV2                                                                 */
+# /*----------------------------------------------------------------------------*/
+# {
+#    V3[0] = V1[1]*V2[2] - V1[2]*V2[1];
+#    V3[1] = V1[2]*V2[0] - V1[0]*V2[2];
+#    V3[2] = V1[0]*V2[1] - V1[1]*V2[0];
+# }
+# 
+# /*----------------------------------------------------------------------------*/
+# static void VectorAdd(float *V1, float *V2, float *V3)
+# /*----------------------------------------------------------------------------*/
+# /* V3 = V1+V2                                                                 */
+# /*----------------------------------------------------------------------------*/
+# {
+#    /* Declarations */
+#    int    i;
+# 
+# 
+#    for (i=0;i<3;i++) V3[i] = V1[i]+V2[i];
+# }
+# 
+# /*----------------------------------------------------------------------------*/
+# static void VectorScale(float *V1, float *K, float *V2)
+# /*----------------------------------------------------------------------------*/
+# /* V3 = V1+V2                                                                 */
+# /*----------------------------------------------------------------------------*/
+# {
+#    /* Declarations */
+#    int    i;
+# 
+# 
+#    for (i=0;i<3;i++) V2[i] = V1[i] * (*K);
+# }
+# #undef MDL_DERIVATIVES
+# #if defined(MDL_DERIVATIVES)
+# /*----------------------------------------------------------------------------*/
+# static void mdlDerivatives(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+# }
+# #endif
+# 
+# #if defined(MATLAB_MEX_FILE) || defined(NRT)
+# #undef MDL_ZERO_CROSSINGS
+#   #if defined(MDL_DERIVATIVES)
+# /*----------------------------------------------------------------------------*/
+# static void mdlZeroCrossings(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+# }
+#   #endif
+# #endif
+# 
+# /*----------------------------------------------------------------------------*/
+# static void mdlTerminate(SimStruct *S)
+# /*----------------------------------------------------------------------------*/
+# {
+# }
+# 
+# #ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
+#   #include "simulink.c"    /* MEX-file interface mechanism */
+# #else
+#   #include "cg_sfun.h"     /* Code generation registration function */
+# #endif
diff --git a/ekf.py b/ekf.py
index 6e5d251..59c1394 100644
--- a/ekf.py
+++ b/ekf.py
@@ -8,6 +8,7 @@
 from utils import safe_tangent, wrap, GRAVITY
 import logging
 import logging.config
+import csv
 
 from truthdata import TruthData
 
@@ -17,6 +18,8 @@
 logging.config.fileConfig("logging.conf")
 logger = logging.getLogger("shumai")
 
+FOUT = csv.writer(open('data.csv', 'w'), delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
+
 ''' EKF = Extended Kalman Filter
 '''
 
@@ -172,9 +175,9 @@ def register_states(self):
                     "Lz":self.Lz,}
         estimates = {"Phi (deg)":degrees(self.phi),
                      "Theta (deg)":degrees(self.theta),}
-        display.register_scalars(scalars, "Sensors")
-        display.register_scalars(estimates, "Estimates")
-        display.register_matrices(matrices, "Internal states")
+        #display.register_scalars(scalars, "Sensors")
+        #display.register_scalars(estimates, "Estimates")
+        #display.register_matrices(matrices, "Internal states")
 
     def estimate_roll_and_pitch(self, p, q, r, Vair, ax, ay, az, dt):
         self.p, self.q, self.r = p, q, r
@@ -274,7 +277,7 @@ def full_kalman_estimate_heading(self, bx, by, bz, phi, theta, q, r, dt):
         self.calc_kalman_gain_matrix()
         self.update_state_estimate(bx, by, bz)
         self.update_covariance_matrix()
-        display.register_scalars({"Psi":180-degrees(self.psi)})
+        #display.register_scalars({"Psi":180-degrees(self.psi)})
         return self.psi
 
     def magnetometer_readings_to_tilt_compensated_heading(self, bx, by, bz, phi, theta):
@@ -295,48 +298,56 @@ def estimate_heading(self, bx, by, bz, phi, theta, q, r, dt):
         psi_error = self.psi_estimate - psi_measured
         self.psi_estimate += self.k_psi * psi_error
         logger.info("Heading %f" % self.psi_estimate)
-        display.register_scalars({"Psi":degrees(self.psi_estimate)})
+        #display.register_scalars({"Psi":degrees(self.psi_estimate)})
         return self.psi_estimate
 
 class PositionObserver:
     def __init__(self):
-        self.Qxt = 0.00025 # noise due to heading error & airspeed
-        self.Qot = 0.00025 # on track noise due to airspeed
+        self.Qxt = 10.0 # noise due to heading error & airspeed
+        self.Qot = 10.0 # on track noise due to airspeed
         self.Wn_est = 0.0
         self.We_est = 0.0
-        self.A = matrix("0,0,-1,0; 0,0,0,-1; 0,0,0,0; 0,0,0,0")
-        self.I = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
-        self.P = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
-        self.Q = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
-        self.L = matrix("1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1")
-        self.C = matrix("1,0,0,0;0,1,0,0")
-        self.X = matrix("0;0;0;0")
-        self.R = matrix("1,0;0,1")
+        self.Cpress = 1.0
+        #self.A = matrix("0,0,-1,0,1.0; 0,0,0,-1,1.0; 0,0,0,0,0; 0,0,0,0,0; 0,0,0,0,0")
+        self.I = matrix("1,0,0,0,0; 0,1,0,0,0; 0,0,1,0,0; 0,0,0,1,0; 0,0,0,0,1")
+        self.P = matrix("1,0,0,0,0; 0,1,0,0,0; 0,0,1,0,0; 0,0,0,1,0; 0,0,0,0,1")
+        self.Q = matrix("1,0,0,0,0; 0,1,0,0,0; 0,0,1,0,0; 0,0,0,1,0; 0,0,0,0,1")
+        self.L = matrix("1,0,0,0,0; 0,1,0,0,0; 0,0,1,0,0; 0,0,0,1,0; 0,0,0,0,1")
+        self.C = matrix("1,0,0,0,0; 0,1,0,0,0")
+        self.X = matrix("0;0;0;0;1.0")
+        self.R = matrix("250.0,0;0,250.0")
 
     def estimate(self, Vair, theta, psi, GPS_Pn, GPS_Pe, dt):
         # CHECK UNITS
+        #psi = wrap(psi)
         psi = wrap(TD.HEADING)
         theta = TD.PITCH
-        Vair = TD.TRUEAIRSPEED * 1.68780986 # m/s to ft/s
+        Vair = TD.AIRSPEED * 1.68780986 # m/s to ft/s
         display.register_scalars({"Vair":Vair,"psi":psi,"theta":theta,"dt":dt},"Dead reckoning")
         Qpn = (self.Qot*abs(cos(psi))) + (self.Qxt*abs(sin(psi)))
         Qpe = (self.Qot*abs(sin(psi))) + (self.Qxt*abs(cos(psi)))
-        Qwn = .00001
-        Qwe = .00001
-        self.Q = matrix("%f,0,0,0; 0,%f,0,0; 0,0,%f,0; 0,0,0,%f" % (Qpn, Qpe, Qwn, Qwe))
-
-        X_dot = matrix("%f;%f;%f;%f" % (Vair*cos(psi)*cos(theta) - self.Wn_est,
-                                        Vair*sin(psi)*cos(theta) - self.We_est,
-                                        0,
-                                        0))
+        Qwn = 1.0
+        Qwe = 1.0
+        Qcpress = 0.1
+        self.Q = matrix("%f,0,0,0,0; 0,%f,0,0,0; 0,0,%f,0,0; 0,0,0,%f,0; 0,0,0,0,%f" % \
+                                                            (Qpn, Qpe, Qwn, Qwe, Qcpress))
+
+        X_dot = matrix("%f;%f;%f;%f;%f" % (self.Cpress*Vair*cos(psi)*cos(theta) - self.Wn_est,
+                                           self.Cpress*Vair*sin(psi)*cos(theta) - self.We_est,
+                                           0,
+                                           0,
+                                           0))
         self.X = self.X+X_dot*dt
 
-        self.A = matrix("0,0,-1,0;0,0,0,-1;0,0,0,0;0,0,0,0")
+        self.A = matrix("0,0,-1,0,%f; 0,0,0,-1,%f; 0,0,0,0,0; 0,0,0,0,0; 0,0,0,0,0" % \
+                                    #(1.0,1.0))
+                                    (Vair*cos(psi)*cos(theta), Vair*cos(theta)*sin(psi)))
+        display.register_scalars({"Vtrue/Vias":(TD.TRUEAIRSPEED*1.68780986)/Vair, "0":Vair*cos(psi)*cos(theta), "1":Vair*cos(theta)*sin(psi)},"Dead reckoning")
 
         P_dot = self.A*self.P + self.P*self.A.transpose() + self.Q
         self.P = self.P + P_dot*dt
 
-        self.C = matrix("1,0,0,0;0,1,0,0")
+        self.C = matrix("1,0,0,0,0; 0,1,0,0,0")
 
         self.L = self.P*self.C.transpose()*linalg.inv(self.R+self.C*self.P*self.C.transpose())
         #To invert the matrix it must not be singular ie have a det()=0
@@ -346,7 +357,27 @@ def estimate(self, Vair, theta, psi, GPS_Pn, GPS_Pe, dt):
         z = matrix("%f;%f" % (GPS_Pn,GPS_Pe))
         h = matrix("%f;%f" % (self.X[0,0],self.X[1,0]))
 
+        display.register_scalars({"Vtrue_est":Vair*self.Cpress/1.68780986},"Dead reckoning")
+        display.register_scalars({"z-h:0":(z-h)[0,0],"z-h:1":(z-h)[1,0]},"Dead reckoning")
+        matrices = {"A":self.A,
+                    "Q":self.Q,
+                    "P":self.P,
+                    "P_dot":P_dot,
+                    "C":self.C,
+                    "L":self.L,
+                    "X":self.X,
+                    "X_dot":X_dot,
+                    "z":z,
+                    "h":h,
+                    "z-h":z-h}
+
         self.X = self.X + self.L*(z-h)
         self.Wn_est, self.We_est = self.X[2,0], self.X[3,0]
-        display.register_scalars({"z-h:0":(z-h)[0,0],"z-h:1":(z-h)[1,0]},"Dead reckoning")
+        self.Cpress = self.X[4,0]
+        display.register_matrices(matrices, "Internal states")
+
+        wind_direction_estimate = degrees(atan2(self.X[3,0], self.X[2,0]))
+        wind_velocity_estimate = sqrt(self.X[3,0]**2 + self.X[2,0]**2) * 0.592483801 # convert from ft/s to knots
+        FOUT.writerow([GPS_Pn, GPS_Pe, self.X[0,0], self.X[1,0], TD.WIND_VELOCITY, TD.WIND_DIRECTION, wind_velocity_estimate, wind_direction_estimate, (TD.TRUEAIRSPEED*1.68780986)/Vair, self.X[4,0]])
+
         return self.X
diff --git a/fgo.py b/fgo.py
index 58c3048..6243f2e 100644
--- a/fgo.py
+++ b/fgo.py
@@ -44,12 +44,12 @@ def estimate(self, theta, psi, Vair, ground_speed, course_over_ground, dt):
         # if they come in as knots
         #Vair = Vair / 0.5144444444
         #ground_speed = ground_speed / 0.5144444444
-        display.register_scalars({"Vair":Vair, "GSPD":ground_speed, "TD.Wdir":TD.WIND_DIRECTION, "TD.Wvel":TD.WIND_VELOCITY}, "Wind FGO")
+        #display.register_scalars({"Vair":Vair, "GSPD":ground_speed, "TD.Wdir":TD.WIND_DIRECTION, "TD.Wvel":TD.WIND_VELOCITY}, "Wind FGO")
         wind_north_error = (Vair * cos(psi) * cos(theta)) - (ground_speed * cos(course_over_ground)) - self.Wn_fgo
         wind_east_error = (Vair * sin(psi) * cos(theta)) - (ground_speed * sin(course_over_ground)) - self.We_fgo
         self.Wn_fgo += self.k * wind_north_error
         self.We_fgo += self.k * wind_east_error
-        display.register_scalars({"CoG":course_over_ground,"psi":psi,"Wn error": wind_north_error, "We error": wind_east_error, "Wn_fgo": self.Wn_fgo, "We_fgo": self.We_fgo, "cos(psi)": cos(psi), "cos(theta)": cos(theta), "sin(psi)": sin(psi), "cos(cog)": cos(course_over_ground), "sin(cog)": sin(course_over_ground), "wind_north_error": wind_north_error, "wind_east_error": wind_east_error}, "Wind FGO")
+        #display.register_scalars({"CoG":course_over_ground,"psi":psi,"Wn error": wind_north_error, "We error": wind_east_error, "Wn_fgo": self.Wn_fgo, "We_fgo": self.We_fgo, "cos(psi)": cos(psi), "cos(theta)": cos(theta), "sin(psi)": sin(psi), "cos(cog)": cos(course_over_ground), "sin(cog)": sin(course_over_ground), "wind_north_error": wind_north_error, "wind_east_error": wind_east_error}, "Wind FGO")
         wind_direction = degrees(atan2(self.We_fgo,self.Wn_fgo))
         wind_velocity = sqrt(self.We_fgo**2 + self.Wn_fgo**2) / 0.5144444444 # convert from m/s to knots
         return wind_direction, wind_velocity
diff --git a/shumai.py b/shumai.py
index 28c59ee..39ffd76 100644
--- a/shumai.py
+++ b/shumai.py
@@ -16,11 +16,15 @@
 from gps import EmulatedXplaneGPS
 from utils import get_ip_address, wrap
 from truthdata import TruthData
+import redis
+
 try:
     from autopilot import Autopilot
 except:
     pass
 
+r = redis.Redis()
+
 def safe_get_ip_address():
     try:
         return get_ip_address()
@@ -39,7 +43,7 @@ def safe_get_ip_address():
           'error': logging.ERROR,
           'critical': logging.CRITICAL}
 
-FOUT = csv.writer(open('data.csv', 'w'), delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
+#FOUT = csv.writer(open('data.csv', 'w'), delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
 
 UDP_PORT=49045 # was 49503
 UDP_SENDTO_PORT=49001
@@ -93,28 +97,28 @@ def loop(self):
         p, q, r = self.imu.read_gyros()
         ax, ay, az = self.imu.read_accelerometers()
         Vair = self.differential_pressure_sensor.read_airspeed() * 0.5144444444 # kias to meters per second
-        display.register_scalars({"Vair":Vair/0.5144444444}, "Sensors")
+        #display.register_scalars({"Vair":Vair/0.5144444444}, "Sensors")
         phi, theta = self.attitude_observer.estimate_roll_and_pitch(p, q, r, Vair, ax, ay, az, TD.DT)
         bx, by, bz = self.magnetometer.read_magnetometer()
         psi = self.heading_observer.estimate_heading(bx, by, bz, phi, theta, q, r, TD.DT)
         gps_data = self.gps.update(TD.DT)
-        display.register_scalars({"gps_lat": gps_data['latitude'],
-                                  "gps_lon": gps_data['longitude'],
-                                  "gps_alt": gps_data['altitude'],
-                                  "gps_sog": gps_data['speed_over_ground'],}, "Sensors")
+        #display.register_scalars({"gps_lat": gps_data['latitude'],
+        #                          "gps_lon": gps_data['longitude'],
+        #                          "gps_alt": gps_data['altitude'],
+        #                          "gps_sog": gps_data['speed_over_ground'],}, "Sensors")
         altitude = self.altitude_observer.estimate(theta, Vair, gps_data['altitude'], TD.DT)
         
-        display.register_scalars({"alt_est": altitude}, "Estimates")
-        display.register_scalars({"Altitude": altitude - TD.ALTITUDE}, "Performance")
+        #display.register_scalars({"alt_est": altitude}, "Estimates")
+        #display.register_scalars({"Altitude": altitude - TD.ALTITUDE}, "Performance")
         wind_direction, wind_velocity = self.wind_observer.estimate(theta, psi, Vair, gps_data['speed_over_ground'] * .5144444444, gps_data['course_over_ground'], TD.DT)
         GPS_Pn, GPS_Pe = self.gps.relative_gps()
         X = self.position_observer.estimate(Vair, theta, psi, GPS_Pn, GPS_Pe, TD.DT)
-        Pn, Pe, Wn, We = X[0,0], X[1,0], X[2,0], X[3,0]
+        Pn, Pe, Wn, We, Cpress = X[0,0], X[1,0], X[2,0], X[3,0], X[4,0]
         wind_direction = degrees(atan2(We,Wn))
         wind_velocity = sqrt(We**2 + Wn**2) # * 0.592483801 # convert from ft/s to knots
-        display.register_scalars({"Pn": Pn,"Pe": Pe,"Wn": Wn,"We": We,"GPS_Pn":GPS_Pn,"GPS_Pe":GPS_Pe}, "Dead reckoning")
+        display.register_scalars({"Pn": Pn,"Pe": Pe,"Wn": Wn,"We": We,"GPS_Pn":GPS_Pn,"GPS_Pe":GPS_Pe,"Cpress":Cpress}, "Dead reckoning")
         display.register_scalars({"wind_direction": wind_direction, "wind_velocity": wind_velocity}, "Dead reckoning")
-        display.register_scalars({"Wdir error": wind_direction - TD.WIND_DIRECTION, "Wvel error": wind_velocity - TD.WIND_VELOCITY}, "Performance")
+        #display.register_scalars({"Wdir error": wind_direction - TD.WIND_DIRECTION, "Wvel error": wind_velocity - TD.WIND_VELOCITY}, "Performance")
         # Shoot, I forget what this is, something from Ryan
         # //SOG[0] = 'data from gps'
         # //u_dot = 'x accel from a2d'
@@ -138,6 +142,8 @@ class XplaneListener(DatagramProtocol):
     def __init__(self):
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         self.sock.bind((safe_get_ip_address(),49998))
+        # self.matlab_graphing_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        # self.matlab_graphing_sock.bind((safe_get_ip_address(),41862))
         self.last_time = datetime.now()
         self.ekf = Shumai(self, self, self, self, self) # hack for X-Plane
         self.bxyz = matrix("0.0 0.0 0.0; 0.0 0.0 0.0; 0.0 0.0 0.0")
@@ -172,23 +178,24 @@ def datagramReceived(self, data, (host, port)):
         TD.ALTITUDE = unpack_from(fmt, data, 9+216+8)[0]
         TD.SPEEDOVERGROUND = unpack_from(fmt, data, 9+12)[0]
         #display.register_scalars({"SOG":TD.SPEEDOVERGROUND, "IAS":TD.AIRSPEED}, "Testing")
-        display.register_scalars({"lat":TD.LATITUDE,"lon":TD.LONGITUDE,"alt":TD.ALTITUDE,"sog":TD.SPEEDOVERGROUND,"cog":degrees(TD.COURSEOVERGROUND)}, "Sensors")
+        #display.register_scalars({"lat":TD.LATITUDE,"lon":TD.LONGITUDE,"alt":TD.ALTITUDE,"sog":TD.SPEEDOVERGROUND,"cog":degrees(TD.COURSEOVERGROUND)}, "Sensors")
         self.generate_virtual_magnetometer_readings(TD.ROLL,TD.PITCH,TD.HEADING)
-        display.register_scalars({"bx":TD.BX,"by":TD.BY,"bz":TD.BZ,"true heading":degrees(TD.HEADING)}, "Sensors")
+        #display.register_scalars({"bx":TD.BX,"by":TD.BY,"bz":TD.BZ,"true heading":degrees(TD.HEADING)}, "Sensors")
         logger.debug("Vair %0.1f, accelerometers (%0.2f, %0.2f, %0.2f), gyros (%0.2f, %0.2f, %0.2f)" % (TD.AIRSPEED, TD.AX, TD.AY, TD.AZ, TD.P, TD.Q, TD.R))
         current_state = self.ekf.loop()
-        display.register_scalars({"Psi error":current_state['yaw']-degrees(TD.HEADING)}, "Performance")
+        #display.register_scalars({"Psi error":current_state['yaw']-degrees(TD.HEADING)}, "Performance")
         if display.curses_available is True:
             display.draw()
         else:
             sys.stdout.write("%sRoll = %f, pitch = %f      " % (chr(13), current_state['roll'], current_state['pitch']))
             sys.stdout.flush()
         #FOUT.writerow([degrees(TD.ROLL), degrees(TD.PITCH), current_state['roll'], current_state['pitch'], current_state['roll'] - degrees(TD.ROLL), current_state['pitch'] - degrees(TD.PITCH)])
-        display.register_scalars({"Phi error": current_state['roll'] - degrees(TD.ROLL), "Theta error": current_state['pitch'] - degrees(TD.PITCH), "Psi error": current_state['yaw'] - degrees(TD.HEADING)}, "Performance")
+        #display.register_scalars({"Phi error": current_state['roll'] - degrees(TD.ROLL), "Theta error": current_state['pitch'] - degrees(TD.PITCH), "Psi error": current_state['yaw'] - degrees(TD.HEADING)}, "Performance")
         try:
             self.autopilot.heading_hold()
             self.sendJoystick((self.autopilot.roll_hold(), self.autopilot.pitch_hold()), 0)
             self.sendThrottle(self.autopilot.throttle())
+            self.sendMatlabGraphingData(TD.DT)
         except:
             pass
 
@@ -203,7 +210,7 @@ def generate_virtual_magnetometer_readings(self, phi, theta, psi):
         self.bxyz[2,0] = (cos(phi) * sin(theta) * cos(psi)) + (sin(phi) * sin(psi))
         self.bxyz[2,1] = (cos(phi) * sin(theta) * sin(psi)) - (sin(phi) * cos(psi))
         self.bxyz[2,2] = cos(phi) * cos(theta)
-        display.register_matrices({"bxyz":self.bxyz})
+        #display.register_matrices({"bxyz":self.bxyz})
         b = self.bxyz * self.mxyz
         TD.BX = b[0,0]
         TD.BY = b[1,0]
@@ -215,8 +222,8 @@ def generate_virtual_magnetometer_readings(self, phi, theta, psi):
         pitch = degrees(pitch)
         roll = degrees(roll)
         yaw = (degrees(yaw) + 360) % 360
-        display.register_scalars({"DCM-pitch": pitch, "DCM-roll": roll, "DCM-yaw": yaw}, "Estimates")
-        display.register_scalars({"DCM-pitch-e": pitch - degrees(TD.PITCH), "DCM-roll-e": roll - degrees(TD.ROLL), "DCM-yaw-e": yaw - degrees(TD.HEADING)}, "Performance")
+        #display.register_scalars({"DCM-pitch": pitch, "DCM-roll": roll, "DCM-yaw": yaw}, "Estimates")
+        #display.register_scalars({"DCM-pitch-e": pitch - degrees(TD.PITCH), "DCM-roll-e": roll - degrees(TD.ROLL), "DCM-yaw-e": yaw - degrees(TD.HEADING)}, "Performance")
 
     def read_gyros(self):
         return TD.P, TD.Q, TD.R
@@ -230,6 +237,12 @@ def read_airspeed(self):
     def read_magnetometer(self):
         return TD.BX, TD.BY, TD.BZ
 
+    def sendMatlabGraphingData(self, dt):
+        r.rpush("xplane", sin(dt))
+        # x = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9,]
+        # data = pack('ffffffffff', x[0], x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8], x[9])
+        # self.matlab_graphing_sock.sendto(data,('70.177.242.58',41862))
+
     def sendThrottle(self, throttle):
         data_selection_packet = "DATA0\x19\x00\x00\x00" # throttle
         data = pack('ffffffff', throttle, throttle, throttle, throttle, throttle, throttle, throttle, throttle)
diff --git a/vgo.py b/vgo.py
index f8a42d1..e3d0746 100644
--- a/vgo.py
+++ b/vgo.py
@@ -54,5 +54,5 @@ def estimate_heading(self, bx, by, bz, phi, theta, q, r, dt):
         psi_error = self.psi_estimate - psi_measured
         self.psi_estimate += self.k_psi * psi_error
         logger.info("Heading %f" % self.psi_estimate)
-        display.register_scalars({"Psi":degrees(self.psi_estimate)}, "Estimates")
+        #display.register_scalars({"Psi":degrees(self.psi_estimate)}, "Estimates")
         return self.psi_estimate