/// <summary>
/// Square Root, Sigma Points Information filter function. This estimates the information (of target) at next timestep (k+1) with given input at time k.
/// </summary>
/// <param name="xPOI">[7 x 1: east north up vel heading turnRate accleration]</param>
/// <param name="xx2">[9 x 1 : 3 NAV, 3ATT, 3GIM] = [E N U, roll pitch yaw, pan tilt scan]</param>
/// <param name="SPOI">[7 x 7 square root covariance of POI]</param>
/// <param name="Sw">[3 x 3 square root covariance of sensor noise: altitude, turnRate, and accleration]</param>
/// <param name="SMeasurement">[2 x 1 covariance vector of measurement]</param>
/// <param name="Sx2">[9 x 9 square root covariance of x2]</param>
/// <param name="zSCR">[2 x 1 Actual measurement of screen (pixels)]</param>
/// <param name="sigma_f">scaling factor for the distance of the sigmap points from the mean</param>
/// <returns></returns>
public void Update(Matrix xPOI, Matrix xx2, Matrix SPOI, Matrix Sw, Matrix SMeasurement, Matrix Sx2, Matrix zSCR, double sigma_f, string screenSize, string cameraType, TargetTypes type)
{
//////////////////////////////// INITIALIZATION /////////////////////////////////////
// SORRY - almost impossible to debug.
// variable names are mostly following IEEE Transaction paper written by Dr. Mark Campbell.
// also the variable names strictly follow the names I used in MATLAB code.
int nPOI = xPOI.Rows;
int nw = Sw.Rows;
int nLaser = 1;
// state matrix
Matrix xHat_a0 = new Matrix(nPOI + nw, 1);
for (int i = 0; i < nPOI; i++)
xHat_a0[i, 0] = xPOI[i, 0];
for (int i = 0; i < nw; i++)
xHat_a0[i + nPOI, 0] = 0;
// covariance matrix
Matrix S_a0 = new Matrix(nPOI + nw, nPOI + nw);
for (int i = 0; i < nPOI; i++)
S_a0[i, i] = SPOI[i, i];
for (int i = nPOI; i < nPOI + nw; i++)
S_a0[i, i] = Sw[i - nPOI, i - nPOI];
// generate 2*(nPOI + nw) + 1 sigma points
Matrix Xa0 = new Matrix(nPOI + nw, 2 * (nPOI + nw) + 1);
Matrix halfSigmaPoints1 = new Matrix(nPOI + nw, nPOI + nw);
Matrix halfSigmaPoints2 = new Matrix(nPOI + nw, nPOI + nw);
Matrix ones = new Matrix(1, nPOI + nw);
ones.Ones();
halfSigmaPoints1 = xHat_a0 * ones + S_a0 * sigma_f;
halfSigmaPoints2 = xHat_a0 * ones - S_a0 * sigma_f;
Xa0.SetSubMatrix(0, nPOI + nw - 1, 0, 0, xHat_a0);
Xa0.SetSubMatrix(0, nPOI + nw - 1, 1, (1 + nPOI + nw) - 1, halfSigmaPoints1);
Xa0.SetSubMatrix(0, nPOI + nw - 1, nPOI + nw + 1, 2 * (nPOI + nw), halfSigmaPoints2);
// dividing into two parts
Matrix XPOI_0 = Xa0.Submatrix(0, nPOI - 1, 0, Xa0.Columns - 1);
Matrix Xw_0 = Xa0.Submatrix(nPOI, Xa0.Rows - 1, 0, Xa0.Columns - 1);
// weight factors
double Wm0 = (sigma_f * sigma_f - (nPOI + nw)) / (sigma_f * sigma_f);
double Wc0 = Wm0 + 3 - sigma_f * sigma_f / (nPOI + nw);
double W = 1 / (2 * sigma_f * sigma_f);
//////////////////////////// SR-SPIF PREDICTION ////////////////////////////////
Matrix predPOIs = predPOI(XPOI_0, Xw_0, dT, type); // predicted sigma points
Matrix xhatPOIm = predPOIs.Submatrix(0, nPOI - 1, 0, 0) * Wm0;
for (int i = 1; i < predPOIs.Columns; i++)
{
xhatPOIm += predPOIs.Submatrix(0, nPOI - 1, i, i) * W;
}
// re-arrange sigma points
Matrix XPOI_Cm = new Matrix(nPOI, 2 * (nPOI + nw) + 1);
for (int i = 0; i < XPOI_Cm.Columns; i++)
{
for (int j = 0; j < XPOI_Cm.Rows; j++)
{
XPOI_Cm[j, i] = predPOIs[j, i] - xhatPOIm[j, 0];
}
}
/////////////////////// CONVERSION FROM PREDICTION TO UPDATE ///////////////////////
// define x_x2
Matrix x_nav = xx2.Submatrix(0, 2, 0, 0);
Matrix x_att = xx2.Submatrix(3, 5, 0, 0);
Matrix x_gim = xx2.Submatrix(6, 8, 0, 0);
Matrix S_nav = Sx2.Submatrix(0, 2, 0, 2);
Matrix S_att = Sx2.Submatrix(3, 5, 3, 5);
Matrix S_gim = Sx2.Submatrix(6, 8, 6, 8);
int nSCR = 2; int nx2 = xx2.Rows; int n2a = nPOI + nw + nSCR + nx2 + nLaser;
Matrix xhat_x2_m = new Matrix(nx2, 1); // creating x_x2 vector
for (int i = 0; i < 3; i++)
{
xhat_x2_m[i, 0] = x_nav[i, 0];
xhat_x2_m[i + 3, 0] = x_att[i, 0];
xhat_x2_m[i + 6, 0] = x_gim[i, 0];
}
Matrix S_x2_m = new Matrix(nx2, nx2); // creating augmented covariance diagonal matrix for x2
for (int i = 0; i < 3; i++)
{
S_x2_m[i, i] = S_nav[i, i];
S_x2_m[i + 3, i + 3] = S_att[i, i];
S_x2_m[i + 6, i + 6] = S_gim[i, i];
}
// equation 20
Matrix X_aug_c0m = new Matrix(nPOI + nSCR + nLaser + nx2, 1);
X_aug_c0m.Zero();
for (int i = 0; i < nPOI; i++)
X_aug_c0m[i, 0] = XPOI_Cm[i, 0];
Matrix X_aug_cm = new Matrix(nPOI + nSCR + nLaser + nx2, 2 * n2a); X_aug_cm.Zero();
X_aug_cm.SetSubMatrix(0, nPOI - 1, 0, XPOI_Cm.Columns - 2, XPOI_Cm.Submatrix(0, nPOI - 1, 1, XPOI_Cm.Columns - 1));
ones = new Matrix(1, 2 * nSCR + nLaser); ones.Ones();
X_aug_cm.SetSubMatrix(0, nPOI - 1, XPOI_Cm.Columns - 1, XPOI_Cm.Columns - 1 + (2 * nSCR + nLaser) - 1, XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * ones);
ones = new Matrix(1, 2 * nx2); ones.Ones();
X_aug_cm.SetSubMatrix(0, nPOI - 1, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser), XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser + nx2) - 1, XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * ones);
X_aug_cm.SetSubMatrix(nPOI, nPOI + nSCR + nLaser - 1, XPOI_Cm.Columns - 1, XPOI_Cm.Columns - 1 + nSCR + nLaser - 1, SMeasurement);
X_aug_cm.SetSubMatrix(nPOI, nPOI + nSCR + nLaser - 1, XPOI_Cm.Columns - 1 + nSCR + nLaser, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser) - 1, SMeasurement * -1);
X_aug_cm.SetSubMatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser), XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser) + nx2 - 1, Sx2);
X_aug_cm.SetSubMatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser) + nx2, XPOI_Cm.Columns - 1 + 2 * (nSCR + nLaser + nx2) - 1, Sx2 * -1);
// equation 21
Matrix X_aug_m = new Matrix(X_aug_c0m.Rows, X_aug_c0m.Columns + X_aug_cm.Columns);
Matrix meanVector = new Matrix(nPOI + nSCR + nLaser + nx2, 1);
meanVector.Zero();
meanVector.SetSubMatrix(0, nPOI - 1, 0, 0, xhatPOIm);
meanVector.SetSubMatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, 0, 0, xx2);
ones = new Matrix(1, 2 * n2a); ones.Ones();
// create augmented sigma points
X_aug_m.SetSubMatrix(0, X_aug_m.Rows - 1, 0, 0, X_aug_c0m + meanVector);
X_aug_m.SetSubMatrix(0, X_aug_m.Rows - 1, 1, (2 * n2a) + 1 - 1, X_aug_cm + meanVector * ones);
// weight configurationU
double Wm0_aug = (sigma_f * sigma_f - n2a) / (sigma_f * sigma_f);
double Wc0_aug = Wm0_aug + 3 - (sigma_f * sigma_f / n2a);
////////////////////////////////////// SR-SPIF UPDATE /////////////////////////////////////////////
Matrix rangeNoise = new Matrix(1, 1); rangeNoise[0, 0] = SMeasurement[2, 2];
Matrix Z_SCR = MeasurementFct(X_aug_m.Submatrix(0, nPOI - 1, 0, X_aug_m.Columns - 1),
X_aug_m.Submatrix(nPOI + nSCR + nLaser, X_aug_m.Rows - 1, 0, X_aug_m.Columns - 1),
X_aug_m.Submatrix(nPOI, nPOI + nSCR + nLaser - 1, 0, X_aug_m.Columns - 1), rangeNoise, cameraType, screenSize);
Matrix z_mean = new Matrix(nSCR + nLaser, 1); // measurement mean
for (int i = 0; i < Z_SCR.Columns; i++)
{
if (i == 0)
z_mean += Z_SCR.Submatrix(0, Z_SCR.Rows - 1, 0, 0) * Wm0_aug;
else
z_mean += Z_SCR.Submatrix(0, Z_SCR.Rows - 1, i, i) * W;
}
ones = new Matrix(1, Z_SCR.Columns); ones.Ones();
Matrix Z_SCR_c = Z_SCR - z_mean * ones;
// equation 27
//Matrix P_POIx2SCR = new Matrix(nPOI + nx2, nSCR);
Matrix augPOIx2cm = new Matrix(nPOI + nx2, X_aug_cm.Columns);
augPOIx2cm.SetSubMatrix(0, nPOI - 1, 0, X_aug_cm.Columns - 1, X_aug_cm.Submatrix(0, nPOI - 1, 0, X_aug_cm.Columns - 1));
augPOIx2cm.SetSubMatrix(nPOI, nPOI + nx2 - 1, 0, X_aug_cm.Columns - 1, X_aug_cm.Submatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, 0, X_aug_cm.Columns - 1));
Matrix augPOIx2c0m = new Matrix(nPOI + nx2, 1);
augPOIx2c0m.SetSubMatrix(0, nPOI - 1, 0, 0, X_aug_c0m.Submatrix(0, nPOI - 1, 0, 0));
augPOIx2c0m.SetSubMatrix(nPOI, nPOI + nx2 - 1, 0, 0, X_aug_c0m.Submatrix(nPOI + nSCR + nLaser, nPOI + nSCR + nLaser + nx2 - 1, 0, 0));
Matrix P_POIx2SCR = augPOIx2cm * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 1, Z_SCR_c.Columns - 1).Transpose() * W
+ augPOIx2c0m * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0).Transpose() * Wc0_aug;
//// Equation 28 and below chol part is equation 15
QrDecomposition qrDecomposition = new QrDecomposition(XPOI_Cm.Submatrix(0, XPOI_Cm.Rows - 1, 1, XPOI_Cm.Columns - 1).Transpose());
Matrix R_X_POI_Cm = qrDecomposition.UpperTriangularFactor;
CholeskyDecomposition cholUpdate;
Matrix S_POI_m;
if (Wc0_aug < 0)
{
cholUpdate = new CholeskyDecomposition(R_X_POI_Cm.Transpose() * R_X_POI_Cm * Math.Sqrt(W) * Math.Sqrt(W) - XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0).Transpose() * Math.Abs(Wc0));
S_POI_m = cholUpdate.LeftTriangularFactor.Transpose();
}
else if (Wc0_aug > 0)
{
cholUpdate = new CholeskyDecomposition(R_X_POI_Cm.Transpose() * R_X_POI_Cm * Math.Sqrt(W) * Math.Sqrt(W) + XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0) * XPOI_Cm.Submatrix(0, nPOI - 1, 0, 0).Transpose() * Math.Abs(Wc0));
S_POI_m = cholUpdate.LeftTriangularFactor.Transpose();
}
else
throw new Exception("Weight is zero");
//Matrix S_POI_mVer2 = new Matrix(nPOI, nPOI);
for (int i = 0; i < nPOI; i++)
{
//S_POI_mVer2[i, i] = Math.Abs(S_POI_m[i, i]);
S_POI_m[i, i] = Math.Abs(S_POI_m[i, i]);
}
QrDecomposition qrSPOImVer2 = new QrDecomposition(S_POI_m.Transpose().Inverse);
Matrix R_POIminus = qrSPOImVer2.UpperTriangularFactor;
Matrix Y_POIminus = R_POIminus.Transpose() * R_POIminus;
//QrDecomposition qrDecomposition_S_x2_m = new QrDecomposition(S_x2_m.Inverse.Transpose());
//Matrix R_x2m = qrDecomposition_S_x2_m.UpperTriangularFactor;
//QrDecomposition qrDecomposition_S_POI_m = new QrDecomposition(S_POI_m.Inverse.Transpose());
//Matrix R_POIm = qrDecomposition_S_POI_m.UpperTriangularFactor;
//Matrix P_POIm1 = R_POIm.Transpose() * R_POIm;
//Matrix P_x2m1 = R_x2m.Transpose() * R_x2m;
//Matrix blkDiag = new Matrix(P_POIm1.Rows + P_x2m1.Rows, P_POIm1.Columns + P_x2m1.Columns);
//blkDiag.SetSubMatrix(0, P_POIm1.Rows - 1, 0, P_POIm1.Columns - 1, P_POIm1);
//blkDiag.SetSubMatrix(P_POIm1.Rows, P_POIm1.Rows + P_x2m1.Rows - 1, P_POIm1.Columns, P_POIm1.Columns + P_x2m1.Columns - 1, P_x2m1);
//Matrix C_SCR = P_POIx2SCR.Transpose() * blkDiag;
// Equation 28
QrDecomposition qrInvSx2 = new QrDecomposition(Sx2.Inverse);
Matrix R_x2minus = qrInvSx2.UpperTriangularFactor;
Matrix Y_x2minus = R_x2minus.Transpose() * R_x2minus;
Matrix blkDiagYseries = new Matrix(nPOI + nx2, nPOI + nx2);
blkDiagYseries.SetSubMatrix(0, nPOI - 1, 0, nPOI - 1, Y_POIminus);
blkDiagYseries.SetSubMatrix(nPOI, nPOI + nx2 - 1, nPOI, nPOI + nx2 - 1, Y_x2minus);
Matrix C = P_POIx2SCR.Transpose() * blkDiagYseries;
// SUPER IMPORTANT STEP !!
QrDecomposition anotherQrDecomposition = new QrDecomposition(Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 1, Z_SCR_c.Columns - 1).Transpose());
Matrix Rvtilde = anotherQrDecomposition.UpperTriangularFactor;
CholeskyDecomposition anotherCholUpdate;
Matrix Svtilde;
if (Wc0_aug < 0)
{
anotherCholUpdate = new CholeskyDecomposition(Rvtilde.Transpose() * Rvtilde * Math.Sqrt(W) * Math.Sqrt(W) - Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0) * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0).Transpose() * Math.Abs(Wc0_aug));
Svtilde = anotherCholUpdate.LeftTriangularFactor.Transpose();
}
else if (Wc0_aug > 0)
{
anotherCholUpdate = new CholeskyDecomposition(Rvtilde.Transpose() * Rvtilde * Math.Sqrt(W) * Math.Sqrt(W) + Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0) * Z_SCR_c.Submatrix(0, Z_SCR_c.Rows - 1, 0, 0).Transpose() * Math.Abs(Wc0_aug));
Svtilde = anotherCholUpdate.LeftTriangularFactor.Transpose();
}
else
throw new Exception("Weight is zero");
//Matrix SvtildeVer2 = new Matrix(Svtilde.Rows, Svtilde.Columns); SvtildeVer2.Zero();
for (int i = 0; i < Svtilde.Columns; i++)
Svtilde[i, i] = Math.Abs(Svtilde[i, i]);
Matrix P_SCR = Svtilde.Transpose() * Svtilde;
Matrix Y_SCR = P_SCR.Inverse;
CholeskyDecomposition lastCholDecompose = new CholeskyDecomposition(Y_SCR);
Matrix R_SCR = lastCholDecompose.LeftTriangularFactor.Transpose();
// Equation 31-33
//QrDecomposition qrDecomposition_SSCR = new QrDecomposition(SSCR.Transpose().Inverse);
//Matrix R_SCR = qrDecomposition_SSCR.UpperTriangularFactor;
Matrix v_kp1 = zSCR - z_mean;
Matrix i_kp1 = new Matrix(nx2 + nSCR + nLaser, 1);
Matrix xhatPOIm_xhat_x2_m_aug = new Matrix(nPOI + nx2, 1);
xhatPOIm_xhat_x2_m_aug.SetSubMatrix(0, nPOI - 1, 0, 0, xhatPOIm);
xhatPOIm_xhat_x2_m_aug.SetSubMatrix(nPOI, nPOI + nx2 - 1, 0, 0, xhat_x2_m);
i_kp1.SetSubMatrix(0, nx2 - 1, 0, 0, R_x2minus * xhat_x2_m);
i_kp1.SetSubMatrix(nx2, nx2 + nSCR + nLaser - 1, 0, 0, R_SCR * (v_kp1 + C * xhatPOIm_xhat_x2_m_aug));
Matrix I_kp1 = R_SCR * C;
// equation 34
// actually [blkDiag; I_kp1];
Matrix blkDiag;
blkDiag = new Matrix(R_POIminus.Rows + R_x2minus.Rows + I_kp1.Rows, R_POIminus.Columns + R_x2minus.Columns);
blkDiag.SetSubMatrix(0, R_POIminus.Rows - 1, 0, R_POIminus.Columns - 1, R_POIminus);
blkDiag.SetSubMatrix(R_POIminus.Rows, R_POIminus.Rows + R_x2minus.Rows - 1, R_POIminus.Columns, R_POIminus.Columns + R_x2minus.Columns - 1, R_x2minus);
blkDiag.SetSubMatrix(R_POIminus.Rows + R_x2minus.Rows, blkDiag.Rows - 1, 0, blkDiag.Columns - 1, I_kp1);
QrDecomposition qrDecomposition_blkDiag = new QrDecomposition(blkDiag);
Matrix R_blob = qrDecomposition_blkDiag.UpperTriangularFactor;
Matrix T_kp1 = qrDecomposition_blkDiag.OrthogonalFactor;
Matrix RPOI = R_blob.Submatrix(0, nPOI - 1, 0, nPOI - 1);
Matrix RPOIx2 = R_blob.Submatrix(0, nPOI - 1, nPOI, R_blob.Columns - 1);
Matrix Rx2 = R_blob.Submatrix(nPOI, R_blob.Rows - 1, nPOI, R_blob.Columns - 1);
// final calculation
Matrix ninvRPOI_RPOIx2_invRx2 = (RPOI.Inverse * -1) * RPOIx2 * Rx2.Inverse;
Matrix R_POIm_xhatPOIm_i_kp1 = new Matrix(nPOI + i_kp1.Rows, 1);
R_POIm_xhatPOIm_i_kp1.SetSubMatrix(0, nPOI - 1, 0, 0, R_POIminus * xhatPOIm);
R_POIm_xhatPOIm_i_kp1.SetSubMatrix(nPOI, R_POIm_xhatPOIm_i_kp1.Rows - 1, 0, 0, i_kp1);
Matrix invRPOI_ninvRPOI_RPOIx2_invRx2 = new Matrix(RPOI.Rows, RPOI.Columns + ninvRPOI_RPOIx2_invRx2.Columns);
invRPOI_ninvRPOI_RPOIx2_invRx2.SetSubMatrix(0, nPOI - 1, 0, nPOI - 1, RPOI.Inverse);
invRPOI_ninvRPOI_RPOIx2_invRx2.SetSubMatrix(0, nPOI - 1, nPOI, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns - 1, ninvRPOI_RPOIx2_invRx2);
Matrix xhatPOI = invRPOI_ninvRPOI_RPOIx2_invRx2 * T_kp1.Transpose() * R_POIm_xhatPOIm_i_kp1;
Matrix invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2 = new Matrix(invRPOI_ninvRPOI_RPOIx2_invRx2.Rows + nx2 + Rx2.Rows, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns);
invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2.SetSubMatrix(0, invRPOI_ninvRPOI_RPOIx2_invRx2.Rows - 1, 0, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns - 1, invRPOI_ninvRPOI_RPOIx2_invRx2);
invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2.SetSubMatrix(invRPOI_ninvRPOI_RPOIx2_invRx2.Rows, invRPOI_ninvRPOI_RPOIx2_invRx2.Rows + nx2 - 1, nPOI, invRPOI_ninvRPOI_RPOIx2_invRx2.Columns - 1, Rx2.Inverse);
QrDecomposition qrDecomposition_invRPOIstuff = new QrDecomposition(invRPOI_ninvRPOI_RPOIx2_invRx2_zeros_invRx2);
Matrix S_blob = qrDecomposition_invRPOIstuff.UpperTriangularFactor;
Matrix newSPOI = S_blob.Submatrix(0, nPOI - 1, 0, nPOI - 1);
targetState = xhatPOI;
if (Math.Abs(newSPOI[0, 0]) < Math.Sqrt(0.05))
newSPOI[0, 0] = Math.Sqrt(0.05) * Math.Sign(newSPOI[0, 0]);
if (Math.Abs(newSPOI[1, 1]) < Math.Sqrt(0.06))
newSPOI[1, 1] = Math.Sqrt(0.05) * Math.Sign(newSPOI[1, 1]);
S_target = newSPOI;
}
public Matrix GenerateEllipse(int numPoint, double centerX, double centerY, Matrix sMatrix, int numSigma)
{
Matrix ep = new Matrix(numPoint, 2); // ellipse points
// generate points in a circle
for (int i = 0; i < numPoint; i++)
{
ep[i, 0] = numSigma * Math.Cos(2 * Math.PI / numPoint * i);
ep[i, 1] = numSigma * Math.Sin(2 * Math.PI / numPoint * i);
}
// skew the ellipse
CholeskyDecomposition chol = new CholeskyDecomposition(sMatrix);
ep = chol.LeftTriangularFactor.Transpose() * ep.Transpose();
ep = ep.Transpose();
// re-center the ellipse
Matrix ones = new Matrix(numPoint, 2);
for (int i = 0; i < numPoint; i++)
{
ep[i, 0] = ep[i, 0] + centerX;
ep[i, 1] = ep[i, 1] + centerY;
}
return ep;
}
void SLAMEKFMainLoop(object o)
{
k = 0;
while (running)
{
lock (dataLock)
{
if (currentScan == null || viconPose == null || odomPose == null || viconLastPose == viconPose)
continue; // no data, no update
else
{
#region discrete-time increment
k = k + 1;
#endregion
if (k == 1)
{
#region state initialization
initialOdomPose = odomPose;
transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp);
transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.yaw = odomPose.yaw;
transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose;
odomLastPose = transformedOdomPose;
xhatvPose = transformedOdomPose;
#endregion
#region map initialization and management
#region extract features -> fz
List<Vector2> currentScanXY = new List<Vector2>();
fz.Clear();
ibook = fExtract(currentScan, bAngle, maxRange);
if (ibook != null)
{
for (int i = 0; i < currentScan.Points.Count; i++)
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
for (int i = 0; i < ibook.Count; i++)
fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose));
}
#endregion
#region populate the map -> landmarkList
landmarkList.Clear();
for (int i = 0; i < fz.Count; i++)
landmarkList.Add(fz[i]);
#endregion
#region populate the map -> mhat
mhat = new Matrix(2 * landmarkList.Count, 1);
for (int i = 0; i < landmarkList.Count; i++)
{
mhat[2 * i, 0] = landmarkList[i].X;
mhat[2 * i + 1, 0] = landmarkList[i].Y;
}
#endregion
#region covariance management -> Paug
Qv = new Matrix(3, 3, Math.Pow(sigw, 2));
Pv = Qv;
Paug = Pv;
for (int i = 0; i < fz.Count; i++)
Paug = blkdiag(Paug, new Matrix(2, 2, Math.Pow(sigw, 2)));
#endregion
lastScan = currentScan;
#endregion
}
else
{
#region odometry processing
transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp);
transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.yaw = odomPose.yaw;
transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose;
xStep = transformedOdomPose - odomLastPose;
double omega = odomLastPose.yaw + xStep.yaw;
if (xStep.x == 0)
uvk.x = 0;
else
{
double vx = xStep.x / Math.Cos(omega);
uvk.x = vx * Math.Cos(omega);
}
if (xStep.y == 0)
uvk.y = 0;
else
{
double vy = xStep.y / Math.Sin(omega);
uvk.y = vy * Math.Sin(omega);
}
uvk.yaw = xStep.yaw;
uvk.z = 0;
uvk.roll = 0;
uvk.pitch = 0;
uvk.timestamp = odomPose.timestamp;
odomLastPose = transformedOdomPose;
#endregion
#region time-update
xhatvPose = xhatvPose + uvk;
Matrix Uvk = new Matrix(3, 1);
Uvk[0, 0] = uvk.x;
Uvk[1, 0] = uvk.y;
Uvk[2, 0] = uvk.yaw;
Pv = Pv + ScalarMultiply(Math.Pow(0.5,2),Uvk * Uvk.Transpose()) + Qv;
Paug.SetSubMatrix(0, 2, 0, 2, Pv);
Matrix Qmv = new Matrix(0, 0, 1.0);
for (int i = 0; i < landmarkList.Count; i++)
Qmv = blkdiag(Qmv, ScalarMultiply(Math.Pow(sigw / 5, 2), Pv.Submatrix(0, 1, 0, 1)));
Matrix Qm = new Matrix(Paug.Rows - 3, Paug.Columns - 3, Math.Pow(sigw / 5, 2));
Paug.SetSubMatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1, Paug.Submatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1) + Qmv + Qm);
#endregion
#region feature exraction
numLandmarks = landmarkList.Count;
List<Vector2> currentScanXY = new List<Vector2>();
fz.Clear();
ibook = fExtract(currentScan, bAngle, maxRange);
if (ibook != null)
{
for (int i = 0; i < currentScan.Points.Count; i++)
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
for (int i = 0; i < ibook.Count; i++)
fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose));
}
#endregion
#region data association
#region variables
fzHat = new List<Vector2>(landmarkList);
List<Vector3> jObsv = new List<Vector3>();
List<Vector2> iObsv = new List<Vector2>();
List<Vector2> obsvFeatureList = new List<Vector2>();
Matrix cLP = new Matrix(fz.Count, fzHat.Count);
Matrix dapGate = new Matrix(fz.Count, fzHat.Count);
Matrix fzM = new Matrix(2,1);
Matrix fzHatM = new Matrix(2,1);
#endregion
#region initialization
// "lpsolver55.dll" directory:
// (note: Alt + F7 -> Build -> enable "Allow UnSafe Code")
lpsolve.Init("C:\\users\\labuser\\desktop\\MAGIC\\Framework\\SLAM");
double[] xOpt;
double ignoreThisEntry = 0;
int nDecision = fz.Count * fzHat.Count;
int nConstraints = fz.Count + fzHat.Count;
int lp = lpsolve.make_lp((int)ignoreThisEntry, nDecision);
//lpsolve.set_sense(lp, true);
#endregion
#region c-vector (objective function)
double[] cObj = new double[nDecision + 1];
cObj[0] = ignoreThisEntry;
int ic = 0;
for (int i = 0; i < fz.Count; i++)
{
for (int j = 0; j < fzHat.Count; j++)
{
ic = ic + 1;
cObj[ic] = -1000000.0 + ciGate(fz[i], Paug.Submatrix(0, 1, 0, 1), fzHat[j], Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4), 1.0);
cLP[i, j] = cObj[ic];
fzM[0,0] = fz[i].X;
fzM[1,0] = fz[i].Y;
fzHatM[0,0] = fzHat[j].X;
fzHatM[1,0] = fzHat[j].Y;
dapGate[i, j] = ((fzM - fzHatM).Transpose() * Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4).Inverse * (fzM - fzHatM))[0, 0];
}
}
lpsolve.set_obj_fn(lp, cObj);
//lpsolve.set_timeout(lp, 0);
#endregion
#region b-vector (RHS of LE)
double[] bVec = new double[nConstraints];
for (int i = 0; i < bVec.Length; i++)
bVec[i] = 1.0;
#endregion
#region A-matrix (constraints setup)
double[,] A = new double[nConstraints, nDecision];
int jc = 0;
for (int i = 0; i < fz.Count; i++)
{
int jr = 1;
for (int j = 0; j < fzHat.Count; j++)
{
A[i, j + jc] = 1;
A[fz.Count - 1 + jr, j + jc] = 1;
jr = jr + 1;
}
jc = jc + fzHat.Count;
}
List<double[]> lpConstraints = new List<double[]>();
lpConstraints.Clear();
for (int i = 0; i < nConstraints; i++)
{
double[] Aline = new double[nDecision + 1];
Aline[0] = ignoreThisEntry;
for (int j = 1; j < nDecision + 1; j++)
{
Aline[j] = A[i, j - 1];
}
lpConstraints.Add(Aline);
}
for (int i = 0; i < nConstraints; i++)
lpsolve.add_constraint(lp, lpConstraints[i], lpsolve.lpsolve_constr_types.LE, bVec[i]);
#endregion
#region optimization and results
lpsolve.solve(lp);
xOpt = new double[lpsolve.get_Ncolumns(lp)];
lpsolve.get_variables(lp, xOpt);
lpsolve.delete_lp(lp);
ic = 0;
double tau = 6.63;
for (int i = 0; i < fz.Count; i++)
{
for (int j = 0; j < fzHat.Count; j++)
{
if ((xOpt[ic] > 0.98) && (xOpt[ic] < 1.02) && (dapGate[i, j] < tau))
jObsv.Add(new Vector3(i, j, cLP[i, j]));
ic = ic + 1;
}
}
if (jObsv.Count > 0)
{
for (int j = 0; j < jObsv.Count; j++)
{
iObsv.Add(new Vector2(jObsv[j].X, jObsv[j].Y));
obsvFeatureList.Add(rotateTranslate(currentScanXY[ibook[(int)(jObsv[j].X)]], xhatvPose)); // dev-only
}
}
numObsv = iObsv.Count;
#endregion
#endregion
#region measurement-update
if (numObsv > 0)
{
#region spf parameters
double L = 3 + 2 * landmarkList.Count;
double alpha = 1.0;
double kappa = 0.0;
double beta = 2.0;
double lambda = Math.Pow(alpha, 2) * (L + kappa) - L;
double gam = Math.Sqrt(L + lambda);
double wm0 = lambda / (L + lambda);
double wc0 = lambda / (L + lambda) + (1 - Math.Pow(alpha, 2) + beta);
wm = new Matrix((int)(2 * L) + 1, 1);
for (int j = 0; j < (int)(2 * L) + 1; j++)
{
if (j == 0)
wm[j, 0] = wm0;
else
wm[j, 0] = 1 / (2.0 * (L + lambda));
}
wc = new Matrix((int)(2 * L) + 1, 1);
for (int j = 0; j < (int)(2.0 * L) + 1; j++)
{
if (j == 0)
wc[j, 0] = wc0;
else
wc[j, 0] = 1 / (2.0 * (L + lambda));
}
#endregion
#region spf sampler
CholeskyDecomposition PCholContainer = new CholeskyDecomposition(ScalarMultiply(L + lambda, Paug));
Matrix PChol = PCholContainer.LeftTriangularFactor;
chi0 = new Matrix((int)L, 1);
chi0[0, 0] = xhatvPose.x - uvk.x;
chi0[1, 0] = xhatvPose.y - uvk.y;
chi0[2, 0] = xhatvPose.yaw - uvk.yaw;
for (int i = 0; i < mhat.Rows; i++)
chi0[3 + i, 0] = mhat[i, 0];
chi1 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, chi0 + PChol.Submatrix(0, chi0.Rows - 1, i, i));
chi2 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, chi0 - PChol.Submatrix(0, chi0.Rows - 1, i, i));
chi = chi0;
chi = chi.Concat(2, chi1);
chi = chi.Concat(2, chi2);
#endregion
#region spf time-update
Matrix chiBar = new Matrix((int)L, (int)(2 * L) + 1);
Matrix uvkSPF = new Matrix((int)L, 1);
uvkSPF.Zero();
uvkSPF[0, 0] = uvk.x;
uvkSPF[1, 0] = uvk.y;
uvkSPF[2, 0] = uvk.yaw;
for (int i = 0; i < (int)(2 * L) + 1; i++)
chiBar.SetSubMatrix(0, chiBar.Rows - 1, i, i, chi.Submatrix(0, chi.Rows - 1, i, i) + uvkSPF);
Matrix xBar = chiBar * wm;
Matrix PBar = new Matrix((int)L, (int)L);
PBar.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
PBar = PBar + ScalarMultiply(wc[i, 0], (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar) * (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar).Transpose());
#endregion
#region sigma-point update
CholeskyDecomposition PBarCholContainer = new CholeskyDecomposition(PBar);
Matrix PBarChol = PBarCholContainer.LeftTriangularFactor;
chi0 = xBar;
chi1 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, xBar + ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i)));
chi2 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, xBar - ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i)));
chiUpdate = chi0;
chiUpdate = chiUpdate.Concat(2, chi1);
chiUpdate = chiUpdate.Concat(2, chi2);
#endregion
#region spf measurement-update setup
#region predicted measurement
Matrix YBar = new Matrix(2 * numObsv, (int)(2 * L) + 1);
YBar.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
{
chiUpdateObsv = new Matrix((int)(2 * numObsv), 1);
for (int j = 0; j < numObsv; j++)
{
chiUpdateObsv[2 * j, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 3), i];
chiUpdateObsv[2 * j + 1, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 1 + 3), i];
}
YBar.SetSubMatrix(0, YBar.Rows - 1, i, i, hFunction(chiUpdate.Submatrix(0, 2, i, i), chiUpdateObsv));
}
Matrix yBar = YBar * wm;
#endregion
#region actual measurement
Matrix z = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
z[2 * i, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.R;
z[2 * i + 1, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.theta;
}
#endregion
#region innovation covariance
S = new Matrix(2 * numObsv, 2 * numObsv);
S.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
S = S + ScalarMultiply(wc[i, 0], (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar) *
(YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose());
S = S + new Matrix(2 * numObsv, 2 * numObsv, Math.Pow(sigv, 2));
#endregion
#region Kalman gain matrix
W = new Matrix((int)L, 2 * numObsv);
W.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
W = W + ScalarMultiply(wc[i, 0], (chiUpdate.Submatrix(0, chiUpdate.Rows - 1, i, i) - xBar) *
(YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose());
W = W * S.Inverse;
#endregion
#region measurement wrapper
zCart = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
zCart[2 * i, 0] = z[2 * i, 0] * Math.Cos(z[2 * i + 1, 0]);
zCart[2 * i + 1, 0] = z[2 * i, 0] * Math.Sin(z[2 * i + 1, 0]);
}
for (int i = 0; i < numObsv; i++)
{
z[2 * i, 0] = Math.Sqrt(Math.Pow(zCart[2 * i, 0], 2) + Math.Pow(zCart[2 * i + 1, 0], 2));
z[2 * i + 1, 0] = Math.Atan2(zCart[2 * i + 1, 0], zCart[2 * i, 0]);
}
yBarCart = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
yBarCart[2 * i, 0] = yBar[2 * i, 0] * Math.Cos(yBar[2 * i + 1, 0]);
yBarCart[2 * i + 1, 0] = yBar[2 * i, 0] * Math.Sin(yBar[2 * i + 1, 0]);
}
for (int i = 0; i < numObsv; i++)
{
yBar[2 * i, 0] = Math.Sqrt(Math.Pow(yBarCart[2 * i, 0], 2) + Math.Pow(yBarCart[2 * i + 1, 0], 2));
yBar[2 * i + 1, 0] = Math.Atan2(yBarCart[2 * i + 1, 0], yBarCart[2 * i, 0]);
}
#endregion
#endregion
#region spf measurement-update
//(((z - yBar).Transpose() * S * (z - yBar))[0, 0] < 6.63)
xBar = xBar + W * (z - yBar);
Paug = PBar - W * S * W.Transpose();
#region SLAM-context of measurement update
#region localization
xhatvPose.x = xBar[0, 0];
xhatvPose.y = xBar[1, 0];
xhatvPose.yaw = xBar[2, 0];
Pv = Paug.Submatrix(0, 2, 0, 2);
#endregion
#region mapping
mhat = xBar.Submatrix(3, (int)L - 1, 0, 0);
int landmarkListCount = landmarkList.Count;
landmarkList.Clear();
for (int i = 0; i < landmarkListCount; i++)
landmarkList.Add(new Vector2(mhat[2 * i, 0], mhat[2 * i + 1, 0]));
#endregion
#endregion
#endregion
}
#endregion
#region show features
slamFeatures.Clear();
if (ibook != null)
{
for (int i = 0; i < ibook.Count; i++)
slamFeatures.Add(fz[i]);
}
#endregion
#region map management
double minDist = 0.5;
double sigMapFreeze = 0.5;
int landmarkMaxCount = 20;
for (int i = 0; i < iObsv.Count; i++)
fz.RemoveAt((int)(iObsv[i].X - i));
for (int i = 0; i < fz.Count; i++)
{
int nFlag = 0;
for (int j = 0; j < landmarkList.Count; j++)
{
if ((fz[i] - landmarkList[j]).Length > minDist)
nFlag = nFlag + 1;
}
if ((nFlag == landmarkList.Count) && (Pv.Trace < sigMapFreeze) && (landmarkList.Count - 1 <= landmarkMaxCount))
{
landmarkList.Add(fz[i]);
Paug = blkdiag(Paug, new Matrix(2, 2, (Pv.Submatrix(0, 1, 0, 1)).Trace));
Matrix mHatNew = new Matrix(2,1);
mHatNew[0, 0] = fz[i].X;
mHatNew[1, 0] = fz[i].Y;
mhat = mhat.Concat(1, mHatNew);
break; // add one feature at a time
}
}
#endregion
#region show landmarks
slamLandmarks.Clear();
slamLandmarksCovariance.Clear();
for (int i = 0; i < landmarkList.Count; i++)
{
slamLandmarks.Add(landmarkList[i]);
slamLandmarksCovariance.Add(Paug.Submatrix(2 * i + 3, 2 * i + 4, 2 * i + 3, 2 * i + 4));
}
#endregion
#region show correspondences
slamCorrespondences.Clear();
for (int i = 0; i < obsvFeatureList.Count; i++)
slamCorrespondences.Add(obsvFeatureList[i]);
#endregion
#region broadcast the results
viconLastPose = viconPose;
//filteredPose = transformedOdomPose;
filteredPose = xhatvPose;
lastScan = currentScan;
//covMatrix = Pv; // only vehicle xy-submatrix is used in the form
covMatrix = Pv; // only vehicle xy-submatrix is used in the form
//transformedOdomPose = viconPose;
#endregion
}
}
}
Thread.Sleep(100);
}
}
