Exemplo n.º 1
0
    private void RunSPKF() {
      // SPKF Steps:
      // 1) Generate Test Points
      // 2) Propagate Test Points
      // 3) Compute Predicted Mean and Covariance
      // 4) Compute Measurements
      // 5) Compute Innovations and Cross Covariance
      // 6) Compute corrections and update

      // Line up initial variables from the controller!
      Double dAlpha = dGreek.get(0);
      Double dBeta = dGreek.get(1);
      cController.setAlpha(dAlpha);
      cController.setBeta(dBeta);
      cController.setKappa(dGreek.get(2));
      Double dGamma = cController.getGamma();
      Double dLambda = cController.getLambda();

      // // DEBUG - Print the Greeks
      // System.out.println("Greeks!");
      // System.out.println("Alpha - " + dAlpha);
      // System.out.println("Beta - " + dBeta);
      // System.out.println("Kappa - " + dGreek.get(2));
      // System.out.println("Lambda - " + dLambda);
      // System.out.println("Gamma - " + dGamma);

      // Let's get started:
      // Step 1: Generate Test Points
      Vector<Matrix> Chi = new Vector<Matrix>();
      Vector<Matrix> UpChi = new Vector<Matrix>();
      Vector<Matrix> UpY = new Vector<Matrix>();
      Matrix UpPx = new Matrix(3, 3, 0.0);
      Matrix UpPy = new Matrix(3, 3, 0.0);
      Matrix UpPxy = new Matrix(3, 3, 0.0);
      Matrix K;
      Vector<Double> wc = new Vector<Double>();
      Vector<Double> wm = new Vector<Double>();
      Chi.add(X); // Add Chi_0 - the current state estimate (X, Y, Z)

      // Big P Matrix is LxL diagonal
      Matrix SqrtP = SqrtSPKF(P);
      SqrtP = SqrtP.times(dGamma);

      // Set up Sigma Points
      for (int i = 0; i <= 8; i++) {
        Matrix tempVec = SqrtP.getMatrix(0, 8, i, i);
        Matrix tempX = X;
        Matrix tempPlus = tempX.plus(tempVec);
        // System.out.println("TempPlus");
        // tempPlus.print(3, 2);
        Matrix tempMinu = tempX.minus(tempVec);
        // System.out.println("TempMinus");
        // tempMinu.print(3, 2);
        // tempX = X.copy();
        // tempX.setMatrix(i, i, 0, 2, tempPlus);
        Chi.add(tempPlus);
        // tempX = X.copy();
        // tempX.setMatrix(i, i, 0, 2, tempMinu);
        Chi.add(tempMinu);
      }

      // DEBUG Print the lines inside the Chi Matrix (2L x L)
      // for (int i = 0; i<=(2*L); i++){
      // System.out.println("Chi Matrix Set: "+i);
      // Chi.get(i).print(5, 2);
      // }

      // Generate weights
      Double WeightZero = (dLambda / (L + dLambda));
      Double OtherWeight = (1 / (2 * (L + dLambda)));
      Double TotalWeight = WeightZero;
      wm.add(WeightZero);
      wc.add(WeightZero + (1 - (dAlpha * dAlpha) + dBeta));
      for (int i = 1; i <= (2 * L); i++) {
        TotalWeight = TotalWeight + OtherWeight;
        wm.add(OtherWeight);
        wc.add(OtherWeight);
      }
      // Weights MUST BE 1 in total
      for (int i = 0; i <= (2 * L); i++) {
        wm.set(i, wm.get(i) / TotalWeight);
        wc.set(i, wc.get(i) / TotalWeight);
      }

      // //DEBUG Print the weights
      // System.out.println("Total Weight:");
      // System.out.println(TotalWeight);
      // for (int i = 0; i<=(2*L); i++){
      // System.out.println("Weight M for "+i+" Entry");
      // System.out.println(wm.get(i));
      // System.out.println("Weight C for "+i+" Entry");
      // System.out.println(wc.get(i));
      // }

      // Step 2: Propagate Test Points
      // This will also handle computing the mean
      Double ux = dControl.elementAt(0);
      Double uy = dControl.elementAt(1);
      Double uz = dControl.elementAt(2);
      Matrix XhatMean = new Matrix(3, 1, 0.0);
      for (int i = 0; i < Chi.size(); i++) {
        Matrix ChiOne = Chi.get(i);
        Matrix Chixminus = new Matrix(3, 1, 0.0);
        Double Xhat = ChiOne.get(0, 0);
        Double Yhat = ChiOne.get(1, 0);
        Double Zhat = ChiOne.get(2, 0);
        Double Xerr = ChiOne.get(3, 0);
        Double Yerr = ChiOne.get(4, 0);
        Double Zerr = ChiOne.get(5, 0);

        Xhat = Xhat + ux + Xerr;
        Yhat = Yhat + uy + Yerr;
        Zhat = Zhat + uz + Zerr;

        Chixminus.set(0, 0, Xhat);
        Chixminus.set(1, 0, Yhat);
        Chixminus.set(2, 0, Zhat);
        // System.out.println("ChixMinus:");
        // Chixminus.print(3, 2);
        UpChi.add(Chixminus);
        XhatMean = XhatMean.plus(Chixminus.times(wm.get(i)));
      }

      // Mean is right!

      // System.out.println("XhatMean: ");
      // XhatMean.print(3, 2);

      // Step 3: Compute Predicted Mean and Covariance
      // Welp, we already solved the mean - let's do the covariance now
      for (int i = 0; i <= (2 * L); i++) {
        Matrix tempP = UpChi.get(i).minus(XhatMean);
        Matrix tempPw = tempP.times(wc.get(i));
        tempP = tempPw.times(tempP.transpose());
        UpPx = UpPx.plus(tempP);
      }

      // New Steps!

      // Step 4: Compute Measurements! (and Y mean!)
      Matrix YhatMean = new Matrix(3, 1, 0.0);
      for (int i = 0; i <= (2 * L); i++) {
        Matrix ChiOne = Chi.get(i);
        Matrix Chiyminus = new Matrix(3, 1, 0.0);
        Double Xhat = UpChi.get(i).get(0, 0);
        Double Yhat = UpChi.get(i).get(1, 0);
        Double Zhat = UpChi.get(i).get(2, 0);
        Double Xerr = ChiOne.get(6, 0);
        Double Yerr = ChiOne.get(7, 0);
        Double Zerr = ChiOne.get(8, 0);

        Xhat = Xhat + Xerr;
        Yhat = Yhat + Yerr;
        Zhat = Zhat + Zerr;

        Chiyminus.set(0, 0, Xhat);
        Chiyminus.set(1, 0, Yhat);
        Chiyminus.set(2, 0, Zhat);
        UpY.add(Chiyminus);
        YhatMean = YhatMean.plus(Chiyminus.times(wm.get(i)));
      }

      // // Welp, we already solved the mean - let's do the covariances
      // now
      // System.out.println("XHatMean and YHatMean = ");
      // XhatMean.print(3, 2);
      // YhatMean.print(3, 2);

      for (int i = 0; i <= (2 * L); i++) {
        Matrix tempPx = UpChi.get(i).minus(XhatMean);
        Matrix tempPy = UpY.get(i).minus(YhatMean);
        // System.out.println("ChiX - XhatMean and ChiY-YhatMean");
        // tempPx.print(3, 2);
        // tempPy.print(3, 2);

        Matrix tempPxw = tempPx.times(wc.get(i));
        Matrix tempPyw = tempPy.times(wc.get(i));

        tempPx = tempPxw.times(tempPy.transpose());
        tempPy = tempPyw.times(tempPy.transpose());
        UpPy = UpPy.plus(tempPy);
        UpPxy = UpPxy.plus(tempPx);
      }

      // Step 6: Compute Corrections and Update

      // Compute Kalman Gain!
      // System.out.println("Updated Px");
      // UpPx.print(5, 2);
      // System.out.println("Updated Py");
      // UpPy.print(5, 2);
      // System.out.println("Updated Pxy");
      // UpPxy.print(5, 2);
      K = UpPxy.times(UpPy.inverse());
      // System.out.println("Kalman");
      // K.print(5, 2);

      Matrix Mea = new Matrix(3, 1, 0.0);
      Mea.set(0, 0, dMeasure.get(0));
      Mea.set(1, 0, dMeasure.get(1));
      Mea.set(2, 0, dMeasure.get(2));

      Matrix Out = K.times(Mea.minus(YhatMean));
      Out = Out.plus(XhatMean);
      // System.out.println("Out:");
      // Out.print(3, 2);

      Matrix Px = UpPx.minus(K.times(UpPy.times(K.transpose())));

      // Update Stuff!
      // Push the P to the controller
      Matrix OutP = P.copy();
      OutP.setMatrix(0, 2, 0, 2, Px);
      X.setMatrix(0, 2, 0, 0, Out);

      Residual = XhatMean.minus(Out);
      cController.inputState(OutP, Residual);
      // cController.setL(L);

      cController.startProcess();
      while (!cController.finishedProcess()) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }

      // System.out.println("Post Greeks: " + cController.getAlpha() + " ,
      // "+ cController.getBeta());

      dGreek.set(0, cController.getAlpha());
      dGreek.set(1, cController.getBeta());
      dGreek.set(2, cController.getKappa());
      P = cController.getP();

      // System.out.println("P is post Process:");
      // P.print(3, 2);

      StepDone = true;
    }
Exemplo n.º 2
0
  public void startProcess() {
    // Generate new Variables
    GPSEntry gGPS;
    IMUEntry iIMU;
    OdoEntry oOdo;
    CompassEntry cComp;
    Double dTS;
    Vector<Double> vIMU = new Vector<Double>();
    Vector<Double> vComp = new Vector<Double>();
    Vector<Double> DeltaH = new Vector<Double>();
    Vector<Double> tempOdo = new Vector<Double>();
    Vector<Double> vGPS = new Vector<Double>();
    Vector<Double> vGreekHeading = new Vector<Double>();
    Vector<Double> vGreekHeading1 = new Vector<Double>();
    Matrix Deviation = new Matrix(2, 1, 0.0);

    // Stores Alpha,
    // Beta and
    // Kappa for
    // each type of
    // controller
    Vector<Double> vGreekDrive = new Vector<Double>(); // Stores Alpha,
    // Beta and Kappa
    // for each type of
    // controller
    Vector<Double> vGreekPose = new Vector<Double>(); // Stores Alpha,
    // Beta and Kappa
    // for each type of
    // controller

    // Start the process!
    ProcessOn = true;
    TimeOn = new Date();
    // Get the length of the dataset
    int iLength = dDataset.getLength();

    // Build the First States
    vGreekHeading.add(defA);
    vGreekHeading.add(defB);
    vGreekHeading.add(defK);
    vGreekDrive = vGreekPose = vGreekHeading1 = vGreekHeading;

    Matrix HeadingX = new Matrix(9, 1, 0.0);
    Matrix HeadingP = new Matrix(9, 9, 0.0);
    Matrix Heading1X = new Matrix(9, 1, 0.0);
    Matrix Heading1P = new Matrix(9, 9, 0.0);
    Matrix DriveX = new Matrix(9, 1, 0.0);
    Matrix DriveP = new Matrix(9, 9, 0.0);
    Matrix PoseX = new Matrix(9, 1, 0.0);
    Matrix PoseP = new Matrix(9, 9, 0.0);

    // Reset the P values to norm
    HeadingP.set(3, 3, dQ);
    HeadingP.set(4, 4, dQ);
    HeadingP.set(5, 5, dQ);
    HeadingP.set(6, 6, dR);
    HeadingP.set(7, 7, dR);
    HeadingP.set(8, 8, dR);

    Heading1P.set(3, 3, dQ);
    Heading1P.set(4, 4, dQ);
    Heading1P.set(5, 5, dQ);
    Heading1P.set(6, 6, dR);
    Heading1P.set(7, 7, dR);
    Heading1P.set(8, 8, dR);

    DriveP.set(3, 3, dQ);
    DriveP.set(4, 4, dQ);
    DriveP.set(5, 5, dQ);
    DriveP.set(6, 6, dR);
    DriveP.set(7, 7, dR);
    DriveP.set(8, 8, dR);

    PoseP.set(3, 3, dQ);
    PoseP.set(4, 4, dQ);
    PoseP.set(5, 5, dQ);
    PoseP.set(6, 6, 2.5);
    PoseP.set(7, 7, 2.5);
    PoseP.set(8, 8, 2.5);

    gGPS = dDataset.GetGPSEntry(0);
    Heading1X.set(2, 0, dDataset.GetOdoEntry(0).getOdoTheta());
    HeadingX.set(2, 0, dDataset.GetCompassEntry(0).GetRad());
    DriveX.set(0, 0, dDataset.GetOdoEntry(0).getOdoX());
    DriveX.set(1, 0, dDataset.GetOdoEntry(0).getOdoY());
    PoseX.set(0, 0, gGPS.getEastX());
    PoseX.set(1, 0, gGPS.getNorthY());

    Double OldTheta = 0.0;
    Double OldGPSX = gGPS.getEastX();
    Double OldGPSY = gGPS.getNorthY();
    Double OldOdoX = 0.0;
    Double OldOdoY = 0.0;
    Double OldComp = dDataset.GetCompassEntry(0).GetRad();
    // Start the roll!
    for (int i = 0; i < iLength; i++) {
      // Get the data @ i
      gGPS = dDataset.GetGPSEntry(i);
      iIMU = dDataset.GetIMUEntry(i);
      oOdo = dDataset.GetOdoEntry(i);
      dTS = gGPS.getGPSTimestamp();
      cComp = dDataset.GetCompassEntry(i);
      // vRIMU = dDataset.GetIMURotVelocity(i);

      // Cascading SPKFs!
      // Sigma Point Kalman Filtering Section

      // Step 1: SPKF IMU Rotation (Acceleration in Robot) with Odo
      // Heading (Delta in World) to get ImpDeltaH
      // Step 2: SPKF ImpDelta (Delta Heading in World) with Heading in
      // World to get TrueHeading
      // Step 3: SPKF IMU Acceleration combined TrueHeading with Odo
      // (Delta Location in World) to get ImpDeltaL
      // Step 4: SPKF ImpDeltaL (Delta Location in World) with GPS
      // Location (Location in World) to get TrueLoc

      // Step 1:
      tempOdo.clear();
      tempOdo.add(0.0);
      tempOdo.add(0.0);
      tempOdo.add(oOdo.getOdoTheta() - OldTheta);
      OldTheta = oOdo.getOdoTheta();
      vIMU.clear();
      vIMU.add(0.0);
      vIMU.add(0.0);
      vIMU.add(iIMU.getIMURotAccel().get(2) * dTS);

      SPKFStep HeadingOne = new SPKFStep(Heading1P, Heading1X, vIMU, tempOdo, vGreekHeading1, dTS);
      while (!HeadingOne.StepFinished()) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }
      Heading1P = HeadingOne.getP();
      Heading1X = HeadingOne.getX();
      vGreekHeading1 = HeadingOne.getGreek();
      // OutputH1.add(new CompassEntry(Heading1X.get(2, 0), dTS, true));
      // ResidH1.add(HeadingOne.getResid());

      // Step 2:
      vComp.clear();
      vComp.add(0.0);
      vComp.add(0.0);
      vComp.add(cComp.GetRad());

      tempOdo.clear();
      tempOdo.add(Heading1X.get(0, 0));
      tempOdo.add(Heading1X.get(1, 0));
      tempOdo.add(Heading1X.get(2, 0));

      // System.out.println("Heading Before at Step " + i + ":");
      // HeadingP.print(3, 2);
      // HeadingX.print(3, 2);
      SPKFStep Heading = new SPKFStep(HeadingP, HeadingX, tempOdo, vComp, vGreekHeading, dTS);
      while (!Heading.StepFinished()) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }
      HeadingP = Heading.getP();
      HeadingX = Heading.getX();
      vGreekHeading = Heading.getGreek();
      // OutputH.add(new CompassEntry(HeadingX.get(2, 0), dTS, true));
      // ResidH.add(Heading.getResid());
      // System.out.println("Heading After at Step " + i + ":");
      // HeadingP.print(3, 2);
      // HeadingX.print(3, 2);
      DeltaH.clear();
      DeltaH.add(HeadingX.get(0, 0));
      DeltaH.add(HeadingX.get(1, 0));
      DeltaH.add(HeadingX.get(2, 0));

      // Step 3:
      vIMU = dDataset.GetIMUVelocity(i, DeltaH);
      for (int j = 0; j < vIMU.size(); j++) {
        vIMU.set(j, (vIMU.get(j) * dTS));
      }
      // This yields Velocity -^

      // Step 3:
      tempOdo.clear();
      tempOdo.add(oOdo.getOdoX() - OldOdoX);
      tempOdo.add(oOdo.getOdoY() - OldOdoY);
      tempOdo.add(0.0);
      OldOdoX = oOdo.getOdoX();
      OldOdoY = oOdo.getOdoY();
      // //
      // System.out.println("Drive Before at Step " + i + ":");
      // DriveP.print(3, 2);
      // DriveX.print(3, 2);
      SPKFStep Drive = new SPKFStep(DriveP, DriveX, vIMU, tempOdo, vGreekDrive, dTS);
      while (!Drive.StepFinished()) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }
      DriveP = Drive.getP();
      DriveX = Drive.getX();
      vGreekDrive = Drive.getGreek();
      // OutputD.add(new OdoEntry(DriveX.get(0, 0), DriveX.get(1, 0),
      // DeltaH.get(2), dTS));
      // ResidD.add(Drive.getResid());
      // System.out.println("Drive After at Step " + i + ":");
      // DriveP.print(3, 2);
      // DriveX.print(3, 2);

      // Step 4:
      tempOdo.clear();
      tempOdo.add(DriveX.get(0, 0));
      tempOdo.add(DriveX.get(1, 0));
      tempOdo.add(DriveX.get(2, 0));
      vGPS.clear();
      vGPS.add(gGPS.getEastX()); // -OldGPSX);
      vGPS.add(gGPS.getNorthY()); // -OldGPSY);
      vGPS.add(0.0);
      if (cController.getControlType() == 2) {
        for (int j = 0; j < vIMU.size(); j++) { // This yields single
          // step motion
          vIMU.set(j, (vIMU.get(j) * dTS));
        }
        Deviation.set(0, 0, gGPS.getEastX() - ((vIMU.get(0) * dTS) + OldGPSX));
        Deviation.set(1, 0, gGPS.getNorthY() - ((vIMU.get(1) * dTS) + OldGPSY));
        cFController = (FuzzySPKF) cController;
        cFController.isGPS(Deviation);
        cController = cFController;
      }

      OldGPSX = gGPS.getEastX();
      OldGPSY = gGPS.getNorthY();

      // System.out.println("Pose Before at Step " + i + ":");
      // PoseP.print(3, 2);
      // PoseX.print(3, 2);
      SPKFStep Pose = new SPKFStep(PoseP, PoseX, tempOdo, vGPS, vGreekPose, dTS);
      while (!Pose.StepFinished()) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }
      PoseP = Pose.getP();
      PoseX = Pose.getX();
      vGreekPose = Pose.getGreek();
      // System.out.println("GPS :" + gGPS.getEastX() + ", " +
      // gGPS.getNorthY());
      // System.out.println("Pose After at Step " + i + ":");
      // PoseP.print(3, 2);
      // PoseX.print(3, 2);
      GPSEntry GPSOut = new GPSEntry(PoseX.get(0, 0), PoseX.get(1, 0), dTS);
      // System.out.println(GPSOut.toString());
      Output.add(GPSOut);
      Resid.add(Pose.getResid());

      // Ticker
      if (i % 100 == 0) System.out.print(".");
    }

    // Process done!
    TimeOff = new Date();
    TimeElapsed = TimeOff.getTime() - TimeOn.getTime();
    ProcessOn = false;
    ProcessDone = true;
  }