From 82fdb0a5f8ccf334a945ea4dd20223590c497e05 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sat, 27 Aug 2011 12:28:47 +0000 Subject: [PATCH] Added ExtendedKalmanFilter class and easyPoint2KalmanFilter example --- examples/Makefile.am | 1 + examples/README | 2 +- examples/easyPoint2KalmanFilter.cpp | 146 +++++++ gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 146 +++++++ gtsam/nonlinear/ExtendedKalmanFilter.h | 69 ++++ gtsam/nonlinear/Makefile.am | 3 + tests/Makefile.am | 1 + tests/testExtendedKalmanFilter.cpp | 426 +++++++++++++++++++++ 8 files changed, 793 insertions(+), 1 deletion(-) create mode 100644 examples/easyPoint2KalmanFilter.cpp create mode 100644 gtsam/nonlinear/ExtendedKalmanFilter-inl.h create mode 100644 gtsam/nonlinear/ExtendedKalmanFilter.h create mode 100644 tests/testExtendedKalmanFilter.cpp diff --git a/examples/Makefile.am b/examples/Makefile.am index 87744e8c0..3c94ea39a 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -19,6 +19,7 @@ noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutoria noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs +noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/README b/examples/README index a27b6b7ed..87368b2ef 100644 --- a/examples/README +++ b/examples/README @@ -6,8 +6,8 @@ Kalman Filter Examples ====================== elaboratePoint2KalmanFilter: simple linear Kalman filter on a moving 2D point, but done using factor graphs easyPoint2KalmanFilter: uses the cool generic templated Kalman filter class to do the same -errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias fullStateKalmanFilter: simple 1D example with a full-state filter +errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias 2D Pose SLAM ============ diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp new file mode 100644 index 000000000..98de6094e --- /dev/null +++ b/examples/easyPoint2KalmanFilter.cpp @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * easyPoint2KalmanFilter.cpp + * + * simple linear Kalman filter on a moving 2D point, but done using factor graphs + * This example uses the templated ExtendedKalmanFilter class to perform the same + * operations as in elaboratePoint2KalmanFilter + * + * Created on: Aug 19, 2011 + * @Author: Frank Dellaert + * @Author: Stephen Williams + */ + +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testExtendedKalmanFilter + * @author Stephen Williams + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Define Types for Linear System Test +typedef TypedSymbol LinearKey; +typedef LieValues LinearValues; +typedef Point2 LinearMeasurement; + +int main() { + + // Create the Kalman Filter initialization point + Point2 x_initial(0.0, 0.0); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + + // Create an ExtendedKalmanFilter object + ExtendedKalmanFilter ekf(x_initial, P_initial); + + + + // Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0) + // In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} + // For the Kalman Filter, this requires a motion model, f(x_{t}) = x_{t+1|t) + // Assuming the system is linear, this will be of the form f(x_{t}) = F*x_{t} + B*u_{t} + w + // where F is the state transition model/matrix, B is the control input model, + // and w is zero-mean, Gaussian white noise with covariance Q + // Note, in some models, Q is actually derived as G*w*G^T where w models uncertainty of some + // physical property, such as velocity or acceleration, and G is derived from physics + // + // For the purposes of this example, let us assume we are using a constant-position model and + // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1] + // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1]. + Vector u = Vector_(2, 1.0, 0.0); + SharedGaussian Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true); + + // This simple motion can be modeled with a BetweenFactor + // Create Keys + LinearKey x0(0), x1(1); + // Predict delta based on controls + Point2 difference(1,0); + // Create Factor + BetweenFactor factor1(x0, x1, difference, Q); + + // Predict the new value with the EKF class + Point2 x1_predict = ekf.predict(factor1); + x1_predict.print("X1 Predict"); + + + + // Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected" + // This is equivalent to saying P(x1|z1) ~ P(z1|x1)*P(x1) + // For the Kalman Filter, this requires a measurement model h(x_{t}) = \hat{z}_{t} + // Assuming the system is linear, this will be of the form h(x_{t}) = H*x_{t} + v + // where H is the observation model/matrix, and v is zero-mean, Gaussian white noise with covariance R + // + // For the purposes of this example, let us assume we have something like a GPS that returns + // the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise + // R = [0.25 0 ; 0 0.25]. + SharedGaussian R = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25), true); + + // This simple measurement can be modeled with a PriorFactor + Point2 z1(1.0, 0.0); + PriorFactor factor2(x1, z1, R); + + // Update the Kalman Filter with the measurement + Point2 x1_update = ekf.update(factor2); + x1_update.print("X1 Update"); + + + + // Do the same thing two more times... + // Predict + LinearKey x2(2); + difference = Point2(1,0); + BetweenFactor factor3(x1, x2, difference, Q); + Point2 x2_predict = ekf.predict(factor1); + x2_predict.print("X2 Predict"); + + // Update + Point2 z2(2.0, 0.0); + PriorFactor factor4(x2, z2, R); + Point2 x2_update = ekf.update(factor4); + x2_update.print("X2 Update"); + + + + // Do the same thing one more time... + // Predict + LinearKey x3(3); + difference = Point2(1,0); + BetweenFactor factor5(x2, x3, difference, Q); + Point2 x3_predict = ekf.predict(factor5); + x3_predict.print("X3 Predict"); + + // Update + Point2 z3(3.0, 0.0); + PriorFactor factor6(x3, z3, R); + Point2 x3_update = ekf.update(factor6); + x3_update.print("X3 Update"); + + return 0; +} diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h new file mode 100644 index 000000000..aa2c9b507 --- /dev/null +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ExtendedKalmanFilter-inl.h + * @brief Class to perform generic Kalman Filtering using nonlinear factor graphs + * @author Stephen Williams + * @author Chris Beall + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +// Function to compare Ordering entries by value instead of by key +bool compareOrderingValues(const Ordering::value_type& i, const Ordering::value_type& j) { + return i.second < j.second; +} + +/* ************************************************************************* */ +template +typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_(const GaussianFactorGraph& linearFactorGraph, + const Ordering& ordering, const VALUES& linearizationPoints, const KEY& lastKey, JacobianFactor::shared_ptr& newPrior) const { + + // Extract the Index of the provided last key + gtsam::Index lastIndex = ordering.at(lastKey); + + // Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) ) + GaussianSequentialSolver solver(linearFactorGraph); + GaussianBayesNet::shared_ptr linearBayesNet = solver.eliminate(); + + // Extract the current estimate of x1,P1 from the Bayes Network + VectorValues result = optimize(*linearBayesNet); + T x = linearizationPoints[lastKey].expmap(result[lastIndex]); + + // Create a Jacobian Factor from the root node of the produced Bayes Net. This will act as a prior for the next iteration. + // The linearization point of this prior must be moved to the new estimate of x, and the key/index needs to be reset to 0, + // the first key in the next iteration + const GaussianConditional::shared_ptr& cg = linearBayesNet->back(); + assert(cg->nrFrontals() == 1); + assert(cg->nrParents() == 0); + // TODO: Find a way to create the correct Jacobian Factor in a single pass + JacobianFactor tmpPrior = JacobianFactor(*cg); + newPrior = JacobianFactor::shared_ptr( + new JacobianFactor(0, tmpPrior.getA(tmpPrior.begin()), tmpPrior.getb() - tmpPrior.getA(tmpPrior.begin()) * result[lastIndex], tmpPrior.get_model())); + + return x; +} + +/* ************************************************************************* */ +template +ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, SharedGaussian P_initial) { + + // Set the initial linearization point to the provided mean + x_ = x_initial; + + // Create a Jacobian Prior Factor directly P_initial. Since x0 is set to the provided mean, the b vector in the prior will be zero + priorFactor_ = JacobianFactor::shared_ptr(new JacobianFactor(0, P_initial->R(), Vector::Zero(x_initial.dim()), noiseModel::Unit::Create(P_initial->dim()))); +} +; + +/* ************************************************************************* */ +template +typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict(const MotionFactor& motionFactor) { + + // TODO: This implementation largely ignores the actual factor symbols. Calling predict() then update() with drastically + // different keys will still compute as if a common key-set was used + + // Create Keys + KEY x0 = motionFactor.key1(); + KEY x1 = motionFactor.key2(); + + // Create an elimination ordering + Ordering ordering; + ordering.insert(x0, 0); + ordering.insert(x1, 1); + + // Create a set of linearization points + VALUES linearizationPoints; + linearizationPoints.insert(x0, x_); + linearizationPoints.insert(x1, x_); + + // Create a Gaussian Factor Graph + GaussianFactorGraph linearFactorGraph; + + // Add in the prior on the first state + linearFactorGraph.push_back(priorFactor_); + + // Linearize motion model and add it to the Kalman Filter graph + linearFactorGraph.push_back(motionFactor.linearize(linearizationPoints, ordering)); + + // Solve the factor graph and update the current state estimate and the prior factor for the next iteration + x_ = solve_(linearFactorGraph, ordering, linearizationPoints, x1, priorFactor_); + + return x_; +} + +/* ************************************************************************* */ +template +typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update(const MeasurementFactor& measurementFactor) { + + // TODO: This implementation largely ignores the actual factor symbols. Calling predict() then update() with drastically + // different keys will still compute as if a common key-set was used + + // Create Keys + KEY x0 = measurementFactor.key(); + + // Create an elimination ordering + Ordering ordering; + ordering.insert(x0, 0); + + // Create a set of linearization points + VALUES linearizationPoints; + linearizationPoints.insert(x0, x_); + + // Create a Gaussian Factor Graph + GaussianFactorGraph linearFactorGraph; + + // Add in the prior on the first state + linearFactorGraph.push_back(priorFactor_); + + // Linearize measurement factor and add it to the Kalman Filter graph + linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoints, ordering)); + + // Solve the factor graph and update the current state estimate and the prior factor for the next iteration + x_ = solve_(linearFactorGraph, ordering, linearizationPoints, x0, priorFactor_); + + return x_; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h new file mode 100644 index 000000000..b08b66233 --- /dev/null +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ExtendedKalmanFilter.h + * @brief Class to perform generic Kalman Filtering using nonlinear factor graphs + * @author Stephen Williams + * @author Chris Beall + */ + +// \callgraph +#pragma once + +#include +#include + +namespace gtsam { + +/** + * This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM + * basically does SRIF with LDL to solve the filter problem, making this an efficient, numerically + * stable Kalman Filter implementation. + * + * The Kalman Filter relies on two models: a motion model that predicts the next state using + * the current state, and a measurement model that predicts the measurement value at a given + * state. Because these two models are situation-dependent, base classes for each have been + * provided above, from which the user must derive a class and incorporate the actual modeling + * equations. A pointer to an instance of each of these classes must be given to the Kalman + * Filter at creation, allowing the Kalman Filter to create factors as needed. + * + * The Kalman Filter class provides a "predict" function and an "update" function to perform + * these steps independently, as well as a "predictAndUpdate" that combines both steps for some + * computational savings. + */ + +template +class ExtendedKalmanFilter { +public: + + typedef boost::shared_ptr > shared_ptr; + typedef typename KEY::Value T; + typedef NonlinearFactor2 MotionFactor; + typedef typename MotionFactor::shared_ptr SharedMotionFactor; + typedef NonlinearFactor1 MeasurementFactor; + typedef typename MeasurementFactor::shared_ptr SharedMeasurementFactor; + +protected: + JacobianFactor::shared_ptr priorFactor_; + T x_; + + T solve_(const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, const VALUES& linearizationPoints, const KEY& x, + JacobianFactor::shared_ptr& newPrior) const; + +public: + ExtendedKalmanFilter(T x_initial, SharedGaussian P_initial); + + T predict(const MotionFactor& motionFactor); + T update(const MeasurementFactor& measurementFactor); +}; + +} // namespace diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 7e9ac429c..777e9e680 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -34,6 +34,9 @@ sources += GaussianISAM2.cpp # Nonlinear constraints headers += NonlinearEquality.h NonlinearConstraint.h +# Kalman Filter +headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am diff --git a/tests/Makefile.am b/tests/Makefile.am index 30e738fa5..8e922aa63 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -22,6 +22,7 @@ check_PROGRAMS += testBoundingConstraint check_PROGRAMS += testNonlinearEqualityConstraint check_PROGRAMS += testPose2SLAMwSPCG check_PROGRAMS += testGaussianISAM2 +check_PROGRAMS += testExtendedKalmanFilter # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp new file mode 100644 index 000000000..0f8666bff --- /dev/null +++ b/tests/testExtendedKalmanFilter.cpp @@ -0,0 +1,426 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testExtendedKalmanFilter + * @author Stephen Williams + */ + +// TODO: Perform tests with nontrivial data +// TODO: Perform tests with nonlinear data + +#include + +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +// Define Types for Linear System Test +typedef TypedSymbol LinearKey; +typedef LieValues LinearValues; +typedef Point2 LinearMeasurement; + +/* ************************************************************************* */ +TEST( ExtendedKalmanFilter, linear ) { + + // Create the Kalman Filter initialization point + Point2 x_initial(0.0, 0.0); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + + // Create an ExtendedKalmanFilter object + ExtendedKalmanFilter ekf(x_initial, P_initial); + + // Create the keys for our example + LinearKey x0(0), x1(1), x2(2), x3(3); + + // Create the controls and measurement properties for our example + double dt = 1.0; + Vector u = Vector_(2, 1.0, 0.0); + Point2 difference(u*dt); + SharedGaussian Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true); + Point2 z1(1.0, 0.0); + Point2 z2(2.0, 0.0); + Point2 z3(3.0, 0.0); + SharedGaussian R = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25), true); + + // Create the set of expected output values + Point2 expected1(1.0, 0.0); + Point2 expected2(2.0, 0.0); + Point2 expected3(3.0, 0.0); + + // Run iteration 1 + // Create motion factor + BetweenFactor factor1(x0, x1, difference, Q); + Point2 predict1 = ekf.predict(factor1); + EXPECT(assert_equal(expected1,predict1)); + + // Create the measurement factor + PriorFactor factor2(x1, z1, R); + Point2 update1 = ekf.update(factor2); + EXPECT(assert_equal(expected1,update1)); + + // Run iteration 2 + BetweenFactor factor3(x1, x2, difference, Q); + Point2 predict2 = ekf.predict(factor3); + EXPECT(assert_equal(expected2,predict2)); + + PriorFactor factor4(x2, z2, R); + Point2 update2 = ekf.update(factor4); + EXPECT(assert_equal(expected2,update2)); + + // Run iteration 3 + BetweenFactor factor5(x2, x3, difference, Q); + Point2 predict3 = ekf.predict(factor5); + EXPECT(assert_equal(expected3,predict3)); + + PriorFactor factor6(x3, z3, R); + Point2 update3 = ekf.update(factor6); + EXPECT(assert_equal(expected3,update3)); + + return; +} + + +// Create Motion Model Factor +class NonlinearMotionModel : public NonlinearFactor2 { +public: + typedef typename LinearKey::Value T; + +private: + typedef NonlinearFactor2 Base; + typedef NonlinearMotionModel This; + +protected: + Matrix Q_; + Matrix Q_invsqrt_; + +public: + NonlinearMotionModel(){} + + NonlinearMotionModel(const LinearKey& key1, const LinearKey& key2) : + Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), key1, key2), Q_(2,2) { + + // Initialize motion model parameters: + // w is vector of motion noise sigmas. The final covariance is calculated as G*w*w'*G' + // TODO: Ultimately, the value Q^-1/2 is needed. This can be calculated/derived symbolically, eliminating the + // need for the noiseModel_, and improving computational performance. + Matrix G(2,2); + Matrix w(2,2); + + G << 1.0, 0.0, 0.0, 1.0; + w << 1.0, 0.0, 0.0, 1.0; + + w = G*w; + Q_ = w*w.transpose(); + Q_invsqrt_ = inverse_square_root(Q_); + } + + virtual ~NonlinearMotionModel() {} + + // Calculate the next state prediction using the nonlinear function f() + T f(const T& x_t0) const { + + // Calculate f(x) + double x = x_t0.x() * x_t0.x(); + double y = x_t0.x() * x_t0.y(); + T x_t1(x,y); + + // In this example, the noiseModel remains constant + return x_t1; + } + + // Calculate the Jacobian of the nonlinear function f() + Matrix F(const T& x_t0) const { + // Calculate Jacobian of f() + Matrix F = Matrix(2,2); + F(0,0) = 2*x_t0.x(); + F(0,1) = 0.0; + F(1,0) = x_t0.y(); + F(1,1) = x_t0.x(); + + return F; + } + + // Calculate the inverse square root of the motion model covariance, Q + Matrix QInvSqrt(const T& x_t0) const { + // This motion model has a constant covariance + return Q_invsqrt_; + } + + /* print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearMotionModel\n"; + std::cout << " key1: " << (std::string) key1_ << std::endl; + std::cout << " key2: " << (std::string) key2_ << std::endl; + } + + /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ + virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { + const This *e = dynamic_cast (&f); + return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_); + } + + /** + * calculate the error of the factor + * Override for systems with unusual noise models + */ + virtual double error(const LinearValues& c) const { + Vector w = whitenedError(c); + return 0.5 * w.dot(w); + } + + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const { + return 2; + } + + /** Vector of errors, whitened ! */ + Vector whitenedError(const LinearValues& c) const { + return QInvSqrt(c[key1_])*unwhitenedError(c); + } + + /** + * Linearize a non-linearFactor2 to get a GaussianFactor + * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z + * Hence b = z - h(x1,x2) = - error_vector(x) + */ + boost::shared_ptr linearize(const LinearValues& c, const Ordering& ordering) const { + const X1& x1 = c[key1_]; + const X2& x2 = c[key2_]; + Matrix A1, A2; + Vector b = -evaluateError(x1, x2, A1, A2); + const Index var1 = ordering[key1_], var2 = ordering[key2_]; + SharedDiagonal constrained = + boost::shared_dynamic_cast(this->noiseModel_); + if (constrained.get() != NULL) { + return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + A2, b, constrained)); + } + // "Whiten" the system before converting to a Gaussian Factor + Matrix Qinvsqrt = QInvSqrt(x1); + A1 = Qinvsqrt*A1; + A2 = Qinvsqrt*A2; + b = Qinvsqrt*b; + return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + A2, b, noiseModel::Unit::Create(b.size()))); + } + + /** vector of errors */ + Vector evaluateError(const T& p1, const T& p2, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + + // error = p2 - f(p1) + // H1 = d error / d p1 = -d f/ d p1 = -F + // H2 = d error / d p2 = I + T prediction = f(p1); + + if(H1){ + *H1 = -F(p1); + } + + if(H2){ + *H2 = eye(dim()); + } + + // Return the error between the prediction and the supplied value of p2 + return prediction.logmap(p2); + } + +}; + +// Create Measurement Model Factor +class NonlinearMeasurementModel : public NonlinearFactor1 { +public: + typedef typename LinearKey::Value T; + +private: + + typedef NonlinearFactor1 Base; + typedef NonlinearMeasurementModel This; + +protected: + Vector z_; /** The measurement */ + Matrix R_; /** The measurement error covariance */ + Matrix R_invsqrt_; /** The inv sqrt of the measurement error covariance */ + +public: + NonlinearMeasurementModel(){} + + NonlinearMeasurementModel(const LinearKey& key, Vector z) : + Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), key), z_(z), R_(1,1) { + + // Initialize nonlinear measurement model parameters: + // z(t) = H*x(t) + v + // where v ~ N(0, noiseModel_) + R_ << 1.0; + R_invsqrt_ = inverse_square_root(R_); + } + + virtual ~NonlinearMeasurementModel() {} + + // Calculate the predicted measurement using the nonlinear function h() + // Byproduct: updates Jacobian H, and noiseModel (R) + Vector h(const T& x_t1) const { + + // Calculate prediction + Vector z_hat(1); + z_hat(0) = 2*x_t1.x()*x_t1.x() - x_t1.x()*x_t1.y() - 2.5*x_t1.x() + 7*x_t1.y(); + + return z_hat; + } + + Matrix H(const T& x_t1) const { + // Update Jacobian + Matrix H(1,2); + H(0,0) = 4*x_t1.x() - x_t1.y() - 2.5; + H(0,1) = -1*x_t1.x() + 7; + + return H; + } + + // Calculate the inverse square root of the motion model covariance, Q + Matrix RInvSqrt(const T& x_t0) const { + // This motion model has a constant covariance + return R_invsqrt_; + } + + /* print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearMeasurementModel\n"; + std::cout << " key: " << (std::string) key_ << std::endl; + } + + /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ + virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { + const This *e = dynamic_cast (&f); + return (e != NULL) && (key_ == e->key_); + } + + /** + * calculate the error of the factor + * Override for systems with unusual noise models + */ + virtual double error(const LinearValues& c) const { + Vector w = whitenedError(c); + return 0.5 * w.dot(w); + } + + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const { + return 1; + } + + /** Vector of errors, whitened ! */ + Vector whitenedError(const LinearValues& c) const { + return RInvSqrt(c[key_])*unwhitenedError(c); + } + + /** + * Linearize a nonlinearFactor1 measurement factor into a GaussianFactor + * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z + * Hence b = z - h(x1) = - error_vector(x) + */ + boost::shared_ptr linearize(const LinearValues& c, const Ordering& ordering) const { + const X& x1 = c[key_]; + Matrix A1; + Vector b = -evaluateError(x1, A1); + const Index var1 = ordering[key_]; + SharedDiagonal constrained = + boost::shared_dynamic_cast(this->noiseModel_); + if (constrained.get() != NULL) { + return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, b, constrained)); + } + // "Whiten" the system before converting to a Gaussian Factor + Matrix Rinvsqrt = RInvSqrt(x1); + A1 = Rinvsqrt*A1; + b = Rinvsqrt*b; + return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, b, noiseModel::Unit::Create(b.size()))); + } + + /** vector of errors */ + Vector evaluateError(const LinearKey::Value& p, boost::optional H1 = boost::none) const { + // error = z - h(p) + // H = d error / d p = -d h/ d p = -H + Vector z_hat = h(p); + + if(H1){ + *H1 = -H(p); + } + + // Return the error between the prediction and the supplied value of p2 + return z_ - z_hat; + } + +}; + + +/* ************************************************************************* */ +TEST( ExtendedKalmanFilter, nonlinear ) { + + // Create the set of expected output values (generated using Matlab Kalman Filter) + Point2 expected_predict[10]; + Point2 expected_update[10]; + expected_predict[1] = Point2(0.81,0.99); + expected_update[1] = Point2(0.824926197027,0.29509808); + expected_predict[2] = Point2(0.680503230541,0.24343413); + expected_update[2] = Point2(0.773360464065,0.43518355); + expected_predict[3] = Point2(0.598086407378,0.33655375); + expected_update[3] = Point2(0.908781566884,0.60766713); + expected_predict[4] = Point2(0.825883936308,0.55223668); + expected_update[4] = Point2(1.23221370495,0.74372849); + expected_predict[5] = Point2(1.51835061468,0.91643243); + expected_update[5] = Point2(1.32823362774,0.855855); + expected_predict[6] = Point2(1.76420456986,1.1367754); + expected_update[6] = Point2(1.36378040243,1.0623832); + expected_predict[7] = Point2(1.85989698605,1.4488574); + expected_update[7] = Point2(1.24068063304,1.3431964); + expected_predict[8] = Point2(1.53928843321,1.6664778); + expected_update[8] = Point2(1.04229257957,1.4856408); + expected_predict[9] = Point2(1.08637382142,1.5484724); + expected_update[9] = Point2(1.13201933157,1.5795507); + expected_predict[10] = Point2(1.28146776705,1.7880819); + expected_update[10] = Point2(1.22159588179,1.7434982); + + // Create the Kalman Filter initialization point + Point2 x_initial(0.90, 1.10); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + + // Create an ExtendedKalmanFilter object + ExtendedKalmanFilter ekf(x_initial, P_initial); + + // Enter Predict-Update Loop + Point2 x_predict, x_update; + for(unsigned int i = 1; i < 9; ++i){ + // Create motion factor + NonlinearMotionModel motionFactor(LinearKey(i-1), LinearKey(i)); + x_predict = ekf.predict(motionFactor); + + // Create a measurement factor + NonlinearMeasurementModel measurementFactor(LinearKey(i), Vector_(1, (double)i)); + x_update = ekf.update(measurementFactor); + + EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); + EXPECT(assert_equal(expected_update[i], x_update, 1e-6)); + } + + return; +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */