void UKF::prediction(const Vector3<>& control, const Matrix2x2<>& R) { Matrix2x2<> L = Covariance::choleskyDecomposition(covariance); std::vector<Vector2<> > sigmaPoints(5); sigmaPoints[0] = (*g)(control, mean); for(int i = 0; i < 2; i++) { sigmaPoints[i + 1] = (*g)(control, mean + L[i]); sigmaPoints[i + 3] = (*g)(control, mean - L[i]); } mean = (sigmaPoints[0] + sigmaPoints[1] + sigmaPoints[2] + sigmaPoints[3] + sigmaPoints[4]) / 5.0f; Matrix2x2<> covG; for(int i = 0; i < 5; i++) { sigmaPoints[i] -= mean; covG[0][0] += sigmaPoints[i][0] * sigmaPoints[i][0]; covG[1][1] += sigmaPoints[i][1] * sigmaPoints[i][1]; const float cov01 = sigmaPoints[i][0] * sigmaPoints[i][1]; covG[1][0] += cov01; covG[0][1] += cov01; } covG /= 2.0f; covariance = covG + R; }
void UKF::update(const Vector2<> measurement, const Matrix2x2<>& Q, const CameraMatrix& theCameraMatrix, const CameraInfo& theCameraInfo, const ImageCoordinateSystem& theImageCoordinateSystem) { Matrix2x2<> L = Covariance::choleskyDecomposition(covariance); std::vector<Vector2<> > sigmaPoints(5); sigmaPoints[0] = (*h)(mean, theCameraMatrix, theCameraInfo, theImageCoordinateSystem); for(int i = 0; i < 2; i++) { sigmaPoints[i + 1] = (*h)(mean + L[i], theCameraMatrix, theCameraInfo, theImageCoordinateSystem); sigmaPoints[i + 3] = (*h)(mean - L[i], theCameraMatrix, theCameraInfo, theImageCoordinateSystem); } Vector2<> meanH = (sigmaPoints[0] + sigmaPoints[1] + sigmaPoints[2] + sigmaPoints[3] + sigmaPoints[4]) / 5.0f; Matrix2x2<> covH; for(int i = 0; i < 5; i++) { sigmaPoints[i] -= meanH; covH[0][0] += sigmaPoints[i][0] * sigmaPoints[i][0]; covH[1][1] += sigmaPoints[i][1] * sigmaPoints[i][1]; const float cov01 = sigmaPoints[i][0] * sigmaPoints[i][1]; covH[0][1] += cov01; covH[1][0] += cov01; } covH /= 2.0f; Matrix2x2<> covHx; for(int i = 1; i < 5; i++) { const float s = i > 2 ? -1.0f : 1.0f; const int Lcol = i % 2 == 1 ? 0 : 1; covHx[0][0] += sigmaPoints[i][0] * s * L[Lcol][0]; covHx[1][1] += sigmaPoints[i][1] * s * L[Lcol][1]; covHx[1][0] += sigmaPoints[i][0] * s * L[Lcol][1]; covHx[0][1] += sigmaPoints[i][1] * s * L[Lcol][0]; } covHx /= 2.0f; const Matrix2x2<> K = covHx.transpose() * (covH + Q).invert(); const Vector2<> innovation = measurement - meanH; mean += K * innovation; covariance -= K * covHx; }
Matrix SRUKF::GenerateSigmaPoints() const { int numberOfSigmaPoints = 2*m_numStates+1; Matrix sigmaPoints(m_mean.getm(), numberOfSigmaPoints, false); sigmaPoints.setCol(0,m_mean); // First sigma point is the current mean with no deviation Matrix deviation; // Matrix sqtCovariance = cholesky(m_numStates / (1-m_sigmaWeights[0][0]) * m_covariance); for(unsigned int i = 1; i < m_numStates + 1; i++){ int negIndex = i+m_numStates; //deviation = sqtCovariance.getCol(i - 1); // Get weighted deviation deviation = m_sigmaSqrtCovWeight*m_sqrtCovariance.getCol(i-1); // Get weighted deviation sigmaPoints.setCol(i, (m_mean + deviation)); // Add mean + deviation sigmaPoints.setCol(negIndex, (m_mean - deviation)); // Add mean - deviation } return sigmaPoints; }
//--------------------------------------------------------------------------------------------------------------------- void UnscentedKalmanFilter::step(const MatrixXd& _Zk, const double _incT) { sigmaPoints(); forecastStep(_incT); dataStep(_Zk); }