14. Generating Sigma Points Assignment 1

Assignment Sigma Point Generation

Instructor Notes:

In the video, the upper left value of sqrt(P_k|k) is 0.00656. It should actually be 0.0656

Generating Sigma Points Assignment

Task Description:

You will be completing the missing code in ukf.cpp, GenerateSigmaPoints() function.

Task List:

Task Feedback:

Nice! Have you tested your code below?

Helpful Equations and Resources

X_{k|k} = \Bigg [ x_{k|k} \qquad x_{k|k}+\sqrt{(\lambda+n_x)P_{k|k}} \qquad x_{k|k}-\sqrt{(\lambda+n_x)P_{k|k}} \Bigg]

remember that \large x_{k|k} is the first column of the Sigma matrix.

\large x_{k|k}+\sqrt{(\lambda+n_x)P_{k|k}} is the second through \large n_x +1 column.

\large x_{k|k}-\sqrt{(\lambda+n_x)P_{k|k}} is the \large n_x +2 column through \large 2n_x +1 column.

and you can set columns easily in a matrix using the .col() function.

Please note that the algorithm used in the quiz (P.llt().matrixL())produces the lower triangular matrix L of the matrix P such that P = L*L^.

Start Quiz:

#include <iostream>
#include "Dense"
#include <vector>
#include "ukf.h"

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::vector;

int main() {

	//Create a UKF instance
	UKF ukf;

/*******************************************************************************
* Programming assignment calls
*******************************************************************************/

    MatrixXd Xsig = MatrixXd(5, 11);
    ukf.GenerateSigmaPoints(&Xsig);

    //print result
    std::cout << "Xsig = " << std::endl << Xsig << std::endl;

	return 0;
}
#include <iostream>
#include "ukf.h"

UKF::UKF() {
  //TODO Auto-generated constructor stub
  Init();
}

UKF::~UKF() {
  //TODO Auto-generated destructor stub
}

void UKF::Init() {

}

/*******************************************************************************
* Programming assignment functions: 
*******************************************************************************/


void UKF::GenerateSigmaPoints(MatrixXd* Xsig_out) {

  //set state dimension
  int n_x = 5;

  //define spreading parameter
  double lambda = 3 - n_x;

  //set example state
  VectorXd x = VectorXd(n_x);
  x <<   5.7441,
         1.3800,
         2.2049,
         0.5015,
         0.3528;

  //set example covariance matrix
  MatrixXd P = MatrixXd(n_x, n_x);
  P <<     0.0043,   -0.0013,    0.0030,   -0.0022,   -0.0020,
          -0.0013,    0.0077,    0.0011,    0.0071,    0.0060,
           0.0030,    0.0011,    0.0054,    0.0007,    0.0008,
          -0.0022,    0.0071,    0.0007,    0.0098,    0.0100,
          -0.0020,    0.0060,    0.0008,    0.0100,    0.0123;

  //create sigma point matrix
  MatrixXd Xsig = MatrixXd(n_x, 2 * n_x + 1);

  //calculate square root of P
  MatrixXd A = P.llt().matrixL();

/*******************************************************************************
 * Student part begin
 ******************************************************************************/

  //your code goes here 
  
  //calculate sigma points ...
  //set sigma points as columns of matrix Xsig

/*******************************************************************************
 * Student part end
 ******************************************************************************/

  //print result
  //std::cout << "Xsig = " << std::endl << Xsig << std::endl;

  //write result
  *Xsig_out = Xsig;

/* expected result:
   Xsig =
    5.7441  5.85768   5.7441   5.7441   5.7441   5.7441  5.63052   5.7441   5.7441   5.7441   5.7441
      1.38  1.34566  1.52806     1.38     1.38     1.38  1.41434  1.23194     1.38     1.38     1.38
    2.2049  2.28414  2.24557  2.29582   2.2049   2.2049  2.12566  2.16423  2.11398   2.2049   2.2049
    0.5015  0.44339 0.631886 0.516923 0.595227   0.5015  0.55961 0.371114 0.486077 0.407773   0.5015
    0.3528 0.299973 0.462123 0.376339  0.48417 0.418721 0.405627 0.243477 0.329261  0.22143 0.286879
*/

}
#ifndef UKF_H
#define UKF_H
#include "Dense"
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;

class UKF {
public:


  /**
	 * Constructor
	 */
	UKF();

	/**
	 * Destructor
	 */
	virtual ~UKF();

	/**
	 * Init Initializes Unscented Kalman filter
	 */
	void Init();

  /**
   * Student assignment functions
   */
  void GenerateSigmaPoints(MatrixXd* Xsig_out);
  void AugmentedSigmaPoints(MatrixXd* Xsig_out);
  void SigmaPointPrediction(MatrixXd* Xsig_out);
  void PredictMeanAndCovariance(VectorXd* x_pred, MatrixXd* P_pred);
  void PredictRadarMeasurement(VectorXd* z_out, MatrixXd* S_out);
  void UpdateState(VectorXd* x_out, MatrixXd* P_out);

};

#endif /* UKF_H */

expected result:

Xsig =

5.7441 5.85768 5.7441 5.7441 5.7441 5.7441 5.63052 5.7441 5.7441 5.7441 5.7441

1.38 1.34566 1.52806 1.38 1.38 1.38 1.41434 1.23194 1.38 1.38 1.38

2.2049 2.28414 2.24557 2.29582 2.2049 2.2049 2.12566 2.16423 2.11398 2.2049 2.2049

0.5015 0.44339 0.631886 0.516923 0.595227 0.5015 0.55961 0.371114 0.486077 0.407773 0.5015

0.3528 0.299973 0.462123 0.376339 0.48417 0.418721 0.405627 0.243477 0.329261 0.22143 0.286879