internal void setReferenceMeasurement(Measurement referenceMeasurement)
 {
     referenceJUNK = referenceMeasurement;
     referenceMeasurementNED = referenceMeasurement.ConvertFromGeodedicToLocalNED(initialReferenceStartingPositionEFEC, initialReferenceStartingPositionGeodedic);
 }
        internal void processMeasurement(Measurement newmeasurementgeodedic, System.IO.StreamWriter file, System.IO.StreamWriter file2)
        {
            //Need to convert new measurement into correct frame.
            //lets do this according to the doc. First we go from geodedic to EFEC, then to inertial, then we do some math
            //and convert to body.
            //so basically get newmeasurement into NED.
            //I also have to ensure that the reference measurement gets properly transformed to local NED

            /**
             * 1. ensure that the new measurement gets transformed to local NED
             * 2. ensure that the first new measurement gets set to previous measurement
             * 3. ensure that the reference measurement gets transformed to local NED
             * 4. Ensure that the takeoff point is transformed to ECEF then used for REF for other conversions to NED
             * 5. make sure appropriate rotations are made for local NED to body frame.
             * 6. ensure body frame still applies because it said stuff about body only being used for velocity.
             */
            Measurement newMeasurementLocalNED = newmeasurementgeodedic.ConvertFromGeodedicToLocalNED(initialReferenceStartingPositionEFEC,
                initialReferenceStartingPositionGeodedic);

            if (firstMeasurement)
            {
                firstMeasurement = false;
                previousMeasurementNED = newMeasurementLocalNED.Copy();
                currentMeasurementNED = newMeasurementLocalNED.Copy();

                return;
            }
            else
            {
                previousMeasurementNED = currentMeasurementNED.Copy();
                currentMeasurementNED = newMeasurementLocalNED.Copy();
            }

               /* Measurement destinationBody = referenceMeasurementNED.Copy();
            destinationBody.Orientation = newMeasurementLocalNED.Orientation;
            destinationBody = destinationBody.ConvertFromLocalNEDToBodyFrame();

            Measurement newMeasurementBody = newMeasurementLocalNED.Copy();
            newMeasurementBody = newMeasurementBody.ConvertFromLocalNEDToBodyFrame();
            */

              //  Position errorPositionBodyFrame = destinationBody.Position.SubtractPosition(newMeasurementBody.Position);

            Position errorPositionNED = referenceMeasurementNED.Position.SubtractPosition(newMeasurementLocalNED.Position);
            Measurement temp = newMeasurementLocalNED.Copy();
            temp.Position = errorPositionNED;
            Position errorPositionBodyFrame = temp.ConvertFromLocalNEDToBodyFrame().Position;
            errorPositionBodyFrame.ZAltitudePosition = errorPositionNED.ZAltitudePosition;

            //*********
            //Calculate integrals
            //**********
            integralOfXBodyLatitude = integralOfXBodyLatitude + errorPositionBodyFrame.XLatitudePosition * Util.TIMEBETWEENINTERVALS;
            //double integralOfYBodyLongitude = calculateIntegral(1);
            integralOfYBodyLongitude = integralOfYBodyLongitude + errorPositionBodyFrame.YLongitudePosition * Util.TIMEBETWEENINTERVALS;
               //     integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;

            Measurement currentmeasurementBodyFrame = currentMeasurementNED.ConvertFromLocalNEDToBodyFrame();
            Measurement previousmeasurementBodyFrame = previousMeasurementNED.ConvertFromLocalNEDToBodyFrame();

            //**********
            //Calculate Velocity
            //**********
               // double xVelocityBodyFrame = (currentmeasurementBodyFrame.Position.XLatitudePosition - previousmeasurementBodyFrame.Position.XLatitudePosition) / Util.TIMEBETWEENINTERVALS;
            //double yVelocityBodyFrame = (currentmeasurementBodyFrame.Position.YLongitudePosition - previousmeasurementBodyFrame.Position.YLongitudePosition) / Util.TIMEBETWEENINTERVALS;

            double xVelocityBodyFrame = currentmeasurementBodyFrame.Velocity.XVelocityBodyFrame;
            double yVelocityBodyFrame = currentmeasurementBodyFrame.Velocity.YVelocityBodyFrame;

            //Note: i'm not using body frame z velocity because the error isn't in body frame. I'm rationalizing this because for z we are not
            //producing an angle.
            //double zVelocityBodyFrame = (currentmeasurementBodyFrame.Position.ZAltitudePosition - previousmeasurementBodyFrame.Position.ZAltitudePosition) / Util.TIMEBETWEENINTERVALS;
              //  double zVelocityBodyFrame = (currentMeasurementNED.Position.ZAltitudePosition - previousMeasurementNED.Position.ZAltitudePosition) / Util.TIMEBETWEENINTERVALS;
            double zVelocityBodyFrame = currentmeasurementBodyFrame.Velocity.ZVelocityBodyFrame;

            //Body and ned comparison works here because the velocity for the reference was already in body frame. (m/s)
            double velocityErrorX = xVelocityBodyFrame - referenceMeasurementNED.Velocity.XVelocityBodyFrame;
            double velocityErrorY = yVelocityBodyFrame - referenceMeasurementNED.Velocity.YVelocityBodyFrame;
            double velocityErrorZ = zVelocityBodyFrame - referenceMeasurementNED.Velocity.ZVelocityBodyFrame;

            //***************
            //Calculate weighted values
            //**************

            //Calculate weighted integrals
            double weightedIntegralOfXBodyLatitude = integralOfXBodyLatitude * Util.K_ETA_X;
            double weightedIntegralOfYBodyLongitude = integralOfYBodyLongitude * Util.K_ETA_Y;

            //    double weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;

            //Calculate weighted proportional values
            double weightedProportionalErrorX = errorPositionBodyFrame.XLatitudePosition * Util.K_X;
            double weightedProportionalErrorY = errorPositionBodyFrame.YLongitudePosition * Util.K_Y;
            double weightedProportionalErrorZ = errorPositionBodyFrame.ZAltitudePosition * Util.K_Z;

            double weightedDerivativeVelocityX = velocityErrorX * Util.K_U;
            double weightedDerivativeVelocityY = velocityErrorY * Util.K_V;
            double weightedDerivativeVelocityZ = velocityErrorZ * Util.K_W;

            /*
            weightedDerivativeVelocityX = 0;
            weightedDerivativeVelocityY = 0;
            weightedDerivativeVelocityZ = 0;
            */

            /* file2.WriteLine("weightedIntegralOfXBodyLatitude," + weightedIntegralOfXBodyLatitude + ",weightedIntegralOfYBodyLongitude," + weightedIntegralOfYBodyLongitude +
                 ",weightedIntegralOfZBodyAltitude," + weightedIntegralOfZBodyAltitude + ",weightedProportionalErrorX," +weightedProportionalErrorX+
                 ",weightedProportionalErrorY," + weightedProportionalErrorY + ",weightedProportionalErrorZ," + weightedProportionalErrorZ+
                 ",weightedDerivativeVelocityX," + weightedDerivativeVelocityX + ",weightedDerivativeVelocityY,"+weightedDerivativeVelocityY+",weightedDerivativeVelocityZ," +
                 weightedDerivativeVelocityZ
                  + ", phi_roll: " + newMeasurementLocalNED.Orientation.Phi_rollDegrees + ", Sai_yaw: " + newMeasurementLocalNED.Orientation.Sai_yawDegrees + ", theta_pitch" + newMeasurementLocalNED.Orientation.Theta_pitchDegrees

                  ); */

            //HACK: CRAP! X CONTROLS THETA WHICH CONTROLS LONGITUDE, NOT LATITUDE.
            Console.Write(weightedIntegralOfXBodyLatitude + ", " + weightedProportionalErrorX + ", " + weightedDerivativeVelocityX + "\r");
            //Calculate Proportional, Integral, Derivative.
            double pidX_thetaDes = weightedIntegralOfXBodyLatitude + weightedProportionalErrorX + weightedDerivativeVelocityX;
            double pidY_phiDes = weightedIntegralOfYBodyLongitude + weightedProportionalErrorY + weightedDerivativeVelocityY;

            double integralOfZBodyAltitudeTemp = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition * Util.TIMEBETWEENINTERVALS;
            double weightedIntegralOfZBodyAltitudeTemp = integralOfZBodyAltitudeTemp * Util.K_ETA_Z;
            double pidZTemp = -1 * weightedIntegralOfZBodyAltitudeTemp - weightedProportionalErrorZ - weightedDerivativeVelocityZ;
            //pidZTemp = -1 * pidZTemp / 10.0;
            pidZTemp = -1 * pidZTemp;
            double pidZ = 0.0;
            double weightedIntegralOfZBodyAltitude = 0.0;

            if (pidZTemp > Util.UCOLMAXCONTROLVALUE || pidZTemp < Util.UCOLMINCONTROLVALUE)
            {
                //integralOfZBodyAltitude = 0;

                if (pidZTemp > Util.UCOLMAXCONTROLVALUE)
                {
                    if (integralOfZBodyAltitude > 0)
                    {
                        integralOfZBodyAltitude = integralOfZBodyAltitude - Util.Ti_Z * (pidZTemp - Util.UCOLMAXCONTROLVALUE);
                    }

                }
                if (pidZTemp < Util.UCOLMINCONTROLVALUE)
                {
                    if (integralOfZBodyAltitude < 0)
                    {
                        integralOfZBodyAltitude = integralOfZBodyAltitude - Util.Ti_Z * (pidZTemp - Util.UCOLMINCONTROLVALUE);
                    }

                }

                //integrator windup preventor
                //integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;
                weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;

                pidZ = -1 * weightedIntegralOfZBodyAltitude - weightedProportionalErrorZ - weightedDerivativeVelocityZ;
                //pidZ = -1 * pidZ / 10.0;
                pidZ = -1 * pidZ;
            }
            else
            {
                //IMPORTANT NOTE: i'M NOT multiplying by time interval because the performance is much better without doing that.
                //integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition * Util.TIMEBETWEENINTERVALS;
                integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;

                weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;
                pidZ = -1 * weightedIntegralOfZBodyAltitude - weightedProportionalErrorZ - weightedDerivativeVelocityZ;
                //pidZ = -1 * pidZ / 10.0;
                pidZ = -1 * pidZ;
            }

            /*if (pidX_thetaDes > 16) pidX_thetaDes = 16;
            if (pidX_thetaDes < -16) pidX_thetaDes = -16;*/
            if (pidX_thetaDes > 13) pidX_thetaDes = 13;
            if (pidX_thetaDes < -13) pidX_thetaDes = -13;
            /*if (pidX_thetaDes > 6) pidX_thetaDes = 6;
            if (pidX_thetaDes < -2) pidX_thetaDes = -2;*
            /*if (pidY_phiDes > 7) pidY_phiDes = 7;
            if (pidY_phiDes < -7) pidY_phiDes = -7;*/
            if (pidY_phiDes > 10) pidY_phiDes = 10;
            if (pidY_phiDes < -10) pidY_phiDes = -10;
              /*  if (pidY_phiDes > 3) pidY_phiDes = 3;
            if (pidY_phiDes < -3) pidY_phiDes = -3;*/

            ulon_MainRotorLongitudinalCyclicControlCommand_Pitch = -1 * ((Util.K_THETA * (newMeasurementLocalNED.Orientation.Theta_pitchDegrees - pidX_thetaDes)) + newMeasurementLocalNED.Velocity.PitchVelocityDeg * Util.K_PitchVelocity);
            ulat_MainRotorLateralCyclicControlCommand_Roll = -1 * ((Util.K_PHI * (newMeasurementLocalNED.Orientation.Phi_rollDegrees + pidY_phiDes)) + (newMeasurementLocalNED.Velocity.RollVelocityDeg * Util.K_RollVelocity));
            ucol_MainRotorCollective_BladeAngle = pidZ;

            file.WriteLine("xlat: ," + newMeasurementLocalNED.Position.XLatitudePosition + ", ylong: ," + newMeasurementLocalNED.Position.YLongitudePosition + ", zalt: ," +
            newMeasurementLocalNED.Position.ZAltitudePosition + ", geolat: ," + newmeasurementgeodedic.Position.XLatitudePosition + ", geolong: ," + newmeasurementgeodedic.Position.YLongitudePosition + ", geoalt: ," + newmeasurementgeodedic.Position.ZAltitudePosition
            + ", Errorx: ," + errorPositionNED.XLatitudePosition + ", Errory: ," + errorPositionNED.YLongitudePosition + ", Errorz ," + errorPositionNED.ZAltitudePosition + ", errorbodx: ," + errorPositionBodyFrame.XLatitudePosition
            + ", errorbody: ," + errorPositionBodyFrame.YLongitudePosition
             + ", errorbodz: ," + errorPositionBodyFrame.ZAltitudePosition
             + ", pidxthetaDes: ," + pidX_thetaDes
             + ", pidY_phiDes: ," + pidY_phiDes
             + ", pixZ: ," + pidZ

             + ", phi_roll: ," + newMeasurementLocalNED.Orientation.Phi_rollDegrees + ", Sai_yaw: ," + newMeasurementLocalNED.Orientation.Sai_yawDegrees + ", theta_pitch," + newMeasurementLocalNED.Orientation.Theta_pitchDegrees
             + ", ulon_pitch: ," + ulon_MainRotorLongitudinalCyclicControlCommand_Pitch
             + ", ulat_roll: ," + ulat_MainRotorLateralCyclicControlCommand_Roll
             + ", collective: ," + ucol_MainRotorCollective_BladeAngle
                 + ", errorbodyx, " + errorPositionBodyFrame.XLatitudePosition
                 + ", errorbodyy," + errorPositionBodyFrame.YLongitudePosition
                 + ", errorbodyz," + errorPositionBodyFrame.ZAltitudePosition
                 + "weightedIntegralOfXBodyLatitude," + weightedIntegralOfXBodyLatitude + ",weightedIntegralOfYBodyLongitude," + weightedIntegralOfYBodyLongitude +
                ",weightedIntegralOfZBodyAltitude," + weightedIntegralOfZBodyAltitude + ",weightedProportionalErrorX," + weightedProportionalErrorX +
                ",weightedProportionalErrorY," + weightedProportionalErrorY + ",weightedProportionalErrorZ," + weightedProportionalErrorZ +
                ",weightedDerivativeVelocityX," + weightedDerivativeVelocityX + ",weightedDerivativeVelocityY," + weightedDerivativeVelocityY + ",weightedDerivativeVelocityZ," +
                weightedDerivativeVelocityZ
                 + ", phi_roll: " + newMeasurementLocalNED.Orientation.Phi_rollDegrees + ", Sai_yaw: " + newMeasurementLocalNED.Orientation.Sai_yawDegrees + ", theta_pitch" + newMeasurementLocalNED.Orientation.Theta_pitchDegrees

             );

            //yawErrors.Enqueue(weightedProportionalErrorX);
               // yawErrors.Enqueue(weightedProportionalErrorZ);
               // yawErrors.Enqueue(errorPositionNED.YLongitudePosition);
            yawErrors.Enqueue(errorPositionNED.XLatitudePosition);
            //    derivativeYaw.Enqueue(weightedDerivativeVelocityY);
             //   derivativeYaw.Enqueue(weightedDerivativeVelocityX);
             //   derivativeYaw.Enqueue(weightedDerivativeVelocityZ);
            derivativeYaw.Enqueue(errorPositionNED.YLongitudePosition);

            //    integralYaw.Enqueue(weightedIntegralOfYBodyLongitude);
             //   integralYaw.Enqueue(weightedIntegralOfYBodyLongitude);
             //   integralYaw.Enqueue(weightedIntegralOfXBodyLatitude);
            //integralYaw.Enqueue(errorPositionNED.ZAltitudePosition);
            integralYaw.Enqueue(errorPositionNED.ZAltitudePosition);

              //  Ped.Enqueue(ulat_MainRotorLateralCyclicControlCommand_Roll);
            Ped.Enqueue(ucol_MainRotorCollective_BladeAngle);

            if (yawErrors.Count > 400)
            {
                yawErrors.Dequeue();
                derivativeYaw.Dequeue();
                integralYaw.Dequeue();
                Ped.Dequeue();
            }
            List<double> myList = new List<double>();
            myList.AddRange(yawErrors);
            Form1.plotLineGraphChart1(myList);
            myList = new List<double>();
            myList.AddRange(derivativeYaw);
            Form1.plotLineGraph(myList, Form1.chart2);
            myList = new List<double>();
            myList.AddRange(integralYaw);
            Form1.plotLineGraph(myList, Form1.chart3);
            myList = new List<double>();
            myList.AddRange(Ped);
            Form1.plotLineGraph(myList, Form1.chart4);

            /**
             * yaw processing
             */
            double yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
            if (yawError > 180)
            {
                yawError = yawError - 360;
            }
            else if (yawError < -180)
            {
                yawError = yawError + 360;
            }

            /*
            if (newMeasurementLocalNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees &&
                previousMeasurementNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees)
            {
                //Boolean going clockwise = false
                isGoingClockwise = false;
            }
            else if (newMeasurementLocalNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees &&
               previousMeasurementNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees)
            {
                isGoingClockwise = true;
            }

            double yawError = 0.0;
            if (isGoingClockwise)
            {
                yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
            }
            else
            {
                yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
            }
            */

            /*if (yawErrors.Count > 40)
            {
                yawErrors.Dequeue();
            }*/

            /**
             * HORRIBLE WAY OF DOING INTEGRAL ANTI-WINDUP
             */

            integralOfYaw = integralOfYaw + yawError * Util.TIMEBETWEENINTERVALS;

            double weightedIntegralOfYawError = integralOfYaw * Util.K_ETA_SAI;
            double weightedProportionalOfYawError = yawError * Util.K_SAI;

            //double derivativeOfYaw = (newMeasurementLocalNED.Orientation.Sai_yawDegrees - previousMeasurementNED.Orientation.Sai_yawDegrees) / Util.TIMEBETWEENINTERVALS;
            double derivativeOfYaw = newMeasurementLocalNED.Velocity.YawVelocity;

            //double yawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity - derivativeOfYaw;
            double yawDerivativeError = derivativeOfYaw - referenceMeasurementNED.Velocity.YawVelocity; //it goes out of control the other way.

            //double weightedYawDerivativeError = yawDerivativeError * Util.K_R;
            //double weightedYawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity * Util.K_R;
            //double weightedYawDerivativeError = currentMeasurementNED.Velocity.YawVelocity * Util.K_R;
            double weightedYawDerivativeError = yawDerivativeError * Util.K_R;

            //control law from: http://books.google.com/books?id=Xqzv_wXqC5EC&pg=PA516&lpg=PA516&dq=pid+controller+tail+rotor&source=bl&ots=PxlTw8tb_Y&sig=u0iBG5m6hV4VeY1yM9ApTPHtO2Q&hl=en#v=onepage&q=pid%20controller%20tail%20rotor&f=false

            //******** note added a * -1 here.
            uped_TailRotorPedal_BladeAngleOnTail = (-1 * weightedIntegralOfYawError - weightedProportionalOfYawError - weightedYawDerivativeError) * -1;

            if (uped_TailRotorPedal_BladeAngleOnTail > Util.UPEDMAXCONTROLVALUE ||
                uped_TailRotorPedal_BladeAngleOnTail < Util.UPEDMINCONTROLVALUE)
            {
                if (uped_TailRotorPedal_BladeAngleOnTail > Util.UPEDMAXCONTROLVALUE)
                {
                    if (integralOfYaw > 0)
                    {
                        integralOfYaw = integralOfYaw - Util.Ti_SAI * (uped_TailRotorPedal_BladeAngleOnTail - Util.UPEDMAXCONTROLVALUE);
                    }

                }
                if (integralOfYaw < Util.UPEDMINCONTROLVALUE)
                {
                    if (integralOfYaw < 0)
                    {
                        integralOfYaw = integralOfYaw - Util.Ti_SAI * (uped_TailRotorPedal_BladeAngleOnTail - Util.UPEDMINCONTROLVALUE);
                    }

                }

             //           integralOfYaw = integralOfYaw - yawError * Util.TIMEBETWEENINTERVALS;
            //    integralOfYaw = 0;
                weightedIntegralOfYawError = integralOfYaw * Util.K_ETA_SAI;

                uped_TailRotorPedal_BladeAngleOnTail = (-1 * weightedIntegralOfYawError - weightedProportionalOfYawError - weightedYawDerivativeError) * -1;
            }

            /**
             * END HORRIBLE WAY OF INTIGRAL ANTIWINDUP
             */

            //            yawErrors.Enqueue(weightedProportionalOfYawError);
               // yawErrors.Enqueue(errorPositionBodyFrame.ZAltitudePosition);
              /*  yawErrors.Enqueue(yawError);
            derivativeYaw.Enqueue(weightedYawDerivativeError);
            integralYaw.Enqueue(weightedIntegralOfYawError);
            Ped.Enqueue(uped_TailRotorPedal_BladeAngleOnTail);

            if (yawErrors.Count > 200)
            {
                yawErrors.Dequeue();
                derivativeYaw.Dequeue();
                integralYaw.Dequeue();
                Ped.Dequeue();
            }
            List<double> myList = new List<double>();
            myList.AddRange(yawErrors);
            Form1.plotLineGraphChart1(myList);
            myList = new List<double>();
            myList.AddRange(derivativeYaw);
            Form1.plotLineGraph(myList, Form1.chart2);
            myList = new List<double>();
            myList.AddRange(integralYaw);
            Form1.plotLineGraph(myList, Form1.chart3);
            myList = new List<double>();
            myList.AddRange(Ped);
            Form1.plotLineGraph(myList, Form1.chart4);
            */

            /*
            yawErrors.Enqueue(yawError);
            derivativeYaw.Enqueue(derivativeOfYaw);
            integralYaw.Enqueue(integralOfYaw);
            Ped.Enqueue(uped_TailRotorPedal_BladeAngleOnTail);

            if (yawErrors.Count > 400)
            {
                yawErrors.Dequeue();
                derivativeYaw.Dequeue();
                integralYaw.Dequeue();
                Ped.Dequeue();
            }

            List<double> myList = new List<double>();
            myList.AddRange(yawErrors);
            Form1.plotLineGraphChart1(myList);
            myList = new List<double>();
            myList.AddRange(derivativeYaw);
            Form1.plotLineGraph(myList, Form1.chart2);
            myList = new List<double>();
            myList.AddRange(integralYaw);
            Form1.plotLineGraph(myList, Form1.chart3);
            myList = new List<double>();
            myList.AddRange(Ped);
            Form1.plotLineGraph(myList, Form1.chart4);
             */

            if (refreshcounter == 5)
            {
                Application.DoEvents();
                refreshcounter = 0;
            }
            else
            {
                refreshcounter++;
            }

               /*     file.WriteLine(newmeasurement.Orientation.Sai_yawDegrees + ", " + referenceMeasurementNED.Orientation.Sai_yawDegrees + ", " +
               yawError + ", " + weightedProportionalOfYawError);*/

               //     Console.Write(weightedIntegralOfZBodyAltitude + ", " + weightedProportionalErrorZ + ", " + weightedDerivativeVelocityZ + "\r");
        }
        internal void processMeasurement(Measurement newmeasurementgeodedic, System.IO.StreamWriter file)
        {
            //Need to convert new measurement into correct frame.
            //lets do this according to the doc. First we go from geodedic to EFEC, then to inertial, then we do some math
            //and convert to body.
            //so basically get newmeasurement into NED.
            //I also have to ensure that the reference measurement gets properly transformed to local NED

            /**
             * 1. ensure that the new measurement gets transformed to local NED
             * 2. ensure that the first new measurement gets set to previous measurement
             * 3. ensure that the reference measurement gets transformed to local NED
             * 4. Ensure that the takeoff point is transformed to ECEF then used for REF for other conversions to NED
             * 5. make sure appropriate rotations are made for local NED to body frame.
             * 6. ensure body frame still applies because it said stuff about body only being used for velocity.
             */
            Measurement newMeasurementLocalNED = newmeasurementgeodedic.ConvertFromGeodedicToLocalNED(initialReferenceStartingPositionEFEC,
                initialReferenceStartingPositionGeodedic);

            if (firstMeasurement)
            {
                firstMeasurement = false;
                previousMeasurementNED = newMeasurementLocalNED.Copy();
                currentMeasurementNED = newMeasurementLocalNED.Copy();

                return;
            }
            else
            {
                previousMeasurementNED = currentMeasurementNED.Copy();
                currentMeasurementNED = newMeasurementLocalNED.Copy();
            }

            Position errorPositionNED = referenceMeasurementNED.Position.SubtractPosition(newMeasurementLocalNED.Position);
            Measurement temp = newMeasurementLocalNED.Copy();
            temp.Position = errorPositionNED;
            Position errorPositionBodyFrame = temp.ConvertFromLocalNEDToBodyFrame().Position;

            //*********
            //Calculate integrals
            //**********
            integralOfXBodyLatitude = integralOfXBodyLatitude + errorPositionBodyFrame.XLatitudePosition;
            //double integralOfYBodyLongitude = calculateIntegral(1);
            integralOfYBodyLongitude = integralOfYBodyLongitude + errorPositionBodyFrame.YLongitudePosition;
            integralOfZBodyAltitude = integralOfZBodyAltitude + errorPositionBodyFrame.ZAltitudePosition;

            Measurement currentmeasurementBodyFrame = currentMeasurementNED.ConvertFromLocalNEDToBodyFrame();
            Measurement previousmeasurementBodyFrame = previousMeasurementNED.ConvertFromLocalNEDToBodyFrame();

            //**********
            //Calculate Velocity
            //**********
            double xVelocityBodyFrame = (currentmeasurementBodyFrame.Position.XLatitudePosition - previousmeasurementBodyFrame.Position.XLatitudePosition) / Util.TIMEBETWEENINTERVALS;
            double yVelocityBodyFrame = (currentmeasurementBodyFrame.Position.YLongitudePosition - previousmeasurementBodyFrame.Position.YLongitudePosition) / Util.TIMEBETWEENINTERVALS;
            double zVelocityBodyFrame = (currentmeasurementBodyFrame.Position.ZAltitudePosition - previousmeasurementBodyFrame.Position.ZAltitudePosition) / Util.TIMEBETWEENINTERVALS;

            //Body and ned comparison works here because the velocity for the reference was already in body frame. (m/s)
            double velocityErrorX = xVelocityBodyFrame - referenceMeasurementNED.Velocity.XVelocityBodyFrame;
            double velocityErrorY = yVelocityBodyFrame - referenceMeasurementNED.Velocity.YVelocityBodyFrame;
            double velocityErrorZ = zVelocityBodyFrame - referenceMeasurementNED.Velocity.ZVelocityBodyFrame;

            //***************
            //Calculate weighted values
            //**************

            //Calculate weighted integrals
            double weightedIntegralOfXBodyLatitude = integralOfXBodyLatitude * Util.K_ETA_X;
            double weightedIntegralOfYBodyLongitude = integralOfYBodyLongitude * Util.K_ETA_Y;
            double weightedIntegralOfZBodyAltitude = integralOfZBodyAltitude * Util.K_ETA_Z;

            //Calculate weighted proportional values
            double weightedProportionalErrorX = errorPositionBodyFrame.XLatitudePosition * Util.K_X;
            double weightedProportionalErrorY = errorPositionBodyFrame.YLongitudePosition * Util.K_Y;
            double weightedProportionalErrorZ = errorPositionBodyFrame.ZAltitudePosition * Util.K_Z;

            double weightedDerivativeVelocityX = velocityErrorX * Util.K_U;
            double weightedDerivativeVelocityY = velocityErrorY * Util.K_V;
            double weightedDerivativeVelocityZ = velocityErrorZ * Util.K_W;

            Console.Write(weightedIntegralOfXBodyLatitude + ", " + weightedProportionalErrorX + ", " + weightedDerivativeVelocityX + "\r");
            //Calculate Proportional, Integral, Derivative.
            double pidX_thetaDes = weightedIntegralOfXBodyLatitude + weightedProportionalErrorX + weightedDerivativeVelocityX;
            double pidY_phiDes = weightedIntegralOfYBodyLongitude + weightedProportionalErrorY + weightedDerivativeVelocityY;
            double pidZ = -1 * weightedIntegralOfZBodyAltitude - weightedProportionalErrorZ - weightedDerivativeVelocityZ;

            ulon_MainRotorLongitudinalCyclicControlCommand_Pitch = -1 * Util.K_THETA * (newMeasurementLocalNED.Orientation.Theta_pitchDegrees - pidX_thetaDes);
            ulat_MainRotorLateralCyclicControlCommand_Roll = -1 * Util.K_PHI * (newMeasurementLocalNED.Orientation.Phi_rollDegrees + pidY_phiDes);
            ucol_MainRotorCollective_BladeAngle = pidZ;

            /**
             * yaw processing
             */
            /*double yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
            if (yawError > 180)
            {
                yawError = yawError - 360;
            }
            else if (yawError < -180)
            {
                yawError = yawError + 360;
            }*/

            //double yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
            //if the current measurement is < ref measure and previous measurement > ref measurement
            //   then you crossed the threshold going the counter clockwise direction
            //       the error should be 0-360 positive
            //else if the current measurement is > ref measurement, and previous measurement < ref measurement
            //  then you crossed the threshold going the clockwise direction.
            //        the error should be 0-360 negative.

            if (newMeasurementLocalNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees &&
                previousMeasurementNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees)
            {
                //Boolean going clockwise = false
                isGoingClockwise = false;
            }else if (newMeasurementLocalNED.Orientation.Sai_yawDegrees > referenceMeasurementNED.Orientation.Sai_yawDegrees &&
                previousMeasurementNED.Orientation.Sai_yawDegrees < referenceMeasurementNED.Orientation.Sai_yawDegrees)
            {
                isGoingClockwise = true;
            }

            double yawError = 0.0;
            if (isGoingClockwise)
            {
                yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
            }
            else
            {
                yawError = newMeasurementLocalNED.Orientation.Sai_yawDegrees - referenceMeasurementNED.Orientation.Sai_yawDegrees;
            }

            yawErrors.Enqueue(yawError);

            if (yawErrors.Count > 40)
            {
                yawErrors.Dequeue();
            }

            integralOfYaw = integralOfYaw + yawError;

            double weightedIntegralOfYawError = integralOfYaw * Util.K_ETA_SAI;
            double weightedProportionalOfYawError = yawError * Util.K_SAI;

            //double derivativeOfYaw = (newMeasurementLocalNED.Orientation.Sai_yawDegrees - previousMeasurementNED.Orientation.Sai_yawDegrees) / Util.TIMEBETWEENINTERVALS;
            double derivativeOfYaw = newMeasurementLocalNED.Velocity.YawVelocity;

            double yawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity - derivativeOfYaw;

            //double weightedYawDerivativeError = yawDerivativeError * Util.K_R;
            double weightedYawDerivativeError = referenceMeasurementNED.Velocity.YawVelocity * Util.K_R;

            //control law from: http://books.google.com/books?id=Xqzv_wXqC5EC&pg=PA516&lpg=PA516&dq=pid+controller+tail+rotor&source=bl&ots=PxlTw8tb_Y&sig=u0iBG5m6hV4VeY1yM9ApTPHtO2Q&hl=en#v=onepage&q=pid%20controller%20tail%20rotor&f=false

            uped_TailRotorPedal_BladeAngleOnTail = -1 * weightedIntegralOfYawError - weightedProportionalOfYawError - weightedYawDerivativeError;

               /*     file.WriteLine(newmeasurement.Orientation.Sai_yawDegrees + ", " + referenceMeasurementNED.Orientation.Sai_yawDegrees + ", " +
               yawError + ", " + weightedProportionalOfYawError);*/

               //     Console.Write(weightedIntegralOfZBodyAltitude + ", " + weightedProportionalErrorZ + ", " + weightedDerivativeVelocityZ + "\r");
        }