Kalman filter now functional, manipulates a functional state.
Frank Dellaert
parent
b1e6f3c526
commit
6df52db2c9
|
|
@ -32,7 +32,7 @@ namespace gtsam {
|
|||
using namespace std;
|
||||
|
||||
/// Auxiliary function to solve factor graph and return pointer to root conditional
|
||||
GaussianConditional::shared_ptr solve(GaussianFactorGraph& factorGraph,
|
||||
KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
|
||||
bool useQR) {
|
||||
|
||||
// Solve the factor graph
|
||||
|
|
@ -41,98 +41,64 @@ namespace gtsam {
|
|||
|
||||
// As this is a filter, all we need is the posterior P(x_t),
|
||||
// so we just keep the root of the Bayes net
|
||||
return bayesNet->back();
|
||||
GaussianConditional::shared_ptr conditional = bayesNet->back();
|
||||
return *conditional;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter::KalmanFilter(size_t n,
|
||||
const GaussianConditional::shared_ptr& density, Factorization method) :
|
||||
n_(n), I_(eye(n_, n_)), method_(method), density_(density) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter KalmanFilter::add(GaussianFactor* newFactor) {
|
||||
KalmanFilter::State fuse(const KalmanFilter::State& p,
|
||||
GaussianFactor* newFactor, bool useQR) {
|
||||
|
||||
// Create a factor graph
|
||||
GaussianFactorGraph factorGraph;
|
||||
|
||||
// push back previous solution and new factor
|
||||
factorGraph.push_back(density_->toFactor());
|
||||
factorGraph.push_back(p.toFactor());
|
||||
factorGraph.push_back(GaussianFactor::shared_ptr(newFactor));
|
||||
|
||||
// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
|
||||
return KalmanFilter(n_, solve(factorGraph, method_ == QR), method_);
|
||||
return solve(factorGraph, useQR);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0,
|
||||
Factorization method) :
|
||||
n_(x0.size()), I_(eye(n_, n_)), method_(method) {
|
||||
KalmanFilter::State KalmanFilter::init(const Vector& x0,
|
||||
const SharedDiagonal& P0) {
|
||||
|
||||
// Create a factor graph f(x0), eliminate it into P(x0)
|
||||
GaussianFactorGraph factorGraph;
|
||||
factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma
|
||||
density_ = solve(factorGraph, method_ == QR);
|
||||
return solve(factorGraph, useQR());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter::KalmanFilter(const Vector& x, const Matrix& P0,
|
||||
Factorization method) :
|
||||
n_(x.size()), I_(eye(n_, n_)), method_(method) {
|
||||
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
|
||||
|
||||
// Create a factor graph f(x0), eliminate it into P(x0)
|
||||
GaussianFactorGraph factorGraph;
|
||||
// 0.5*(x-x0)'*inv(Sigma)*(x-x0)
|
||||
HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0));
|
||||
factorGraph.push_back(factor);
|
||||
density_ = solve(factorGraph, method_ == QR);
|
||||
return solve(factorGraph, useQR());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void KalmanFilter::print(const string& s) const {
|
||||
cout << s << "\n";
|
||||
density_->print("density: ");
|
||||
Vector m = mean();
|
||||
Matrix P = covariance();
|
||||
gtsam::print(m, "mean: ");
|
||||
gtsam::print(P, "covariance: ");
|
||||
cout << s << ", " << n_ << "-dimensional Kalman filter\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector KalmanFilter::mean() const {
|
||||
// Solve for mean
|
||||
VectorValues x;
|
||||
Index k = step();
|
||||
// a VectorValues that only has a value for k: cannot be printed
|
||||
x.insert(k, Vector(n_));
|
||||
density_->rhs(x);
|
||||
density_->solveInPlace(x);
|
||||
return x[k];
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix KalmanFilter::information() const {
|
||||
return density_->computeInformation();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix KalmanFilter::covariance() const {
|
||||
return inverse(information());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter KalmanFilter::predict(const Matrix& F, const Matrix& B,
|
||||
const Vector& u, const SharedDiagonal& model) {
|
||||
KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
|
||||
const Matrix& B, const Vector& u, const SharedDiagonal& model) {
|
||||
|
||||
// The factor related to the motion model is defined as
|
||||
// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
|
||||
Index k = step();
|
||||
return add(new JacobianFactor(k, -F, k+1, I_, B * u, model));
|
||||
Index k = step(p);
|
||||
return fuse(p, new JacobianFactor(k, -F, k + 1, I_, B * u, model), useQR());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter KalmanFilter::predictQ(const Matrix& F, const Matrix& B,
|
||||
const Vector& u, const Matrix& Q) {
|
||||
KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
|
||||
const Matrix& B, const Vector& u, const Matrix& Q) {
|
||||
|
||||
#ifndef NDEBUG
|
||||
int n = F.cols();
|
||||
|
|
@ -151,27 +117,28 @@ namespace gtsam {
|
|||
Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
|
||||
Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
|
||||
double f = dot(b, g2);
|
||||
Index k = step();
|
||||
return add(new HessianFactor(k, k+1, G11, G12, g1, G22, g2, f));
|
||||
Index k = step(p);
|
||||
return fuse(p, new HessianFactor(k, k + 1, G11, G12, g1, G22, g2, f),
|
||||
useQR());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter KalmanFilter::predict2(const Matrix& A0, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
|
||||
const Matrix& A1, const Vector& b, const SharedDiagonal& model) {
|
||||
// Nhe factor related to the motion model is defined as
|
||||
// f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
|
||||
Index k = step();
|
||||
return add(new JacobianFactor(k, A0, k+1, A1, b, model));
|
||||
Index k = step(p);
|
||||
return fuse(p, new JacobianFactor(k, A0, k + 1, A1, b, model), useQR());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
KalmanFilter KalmanFilter::update(const Matrix& H, const Vector& z,
|
||||
const SharedDiagonal& model) {
|
||||
KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
|
||||
const Vector& z, const SharedDiagonal& model) {
|
||||
// The factor related to the measurements would be defined as
|
||||
// f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
|
||||
// = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
|
||||
Index k = step();
|
||||
return add(new JacobianFactor(k, H, z, model));
|
||||
Index k = step(p);
|
||||
return fuse(p, new JacobianFactor(k, H, z, model), useQR());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -20,8 +20,7 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianDensity.h>
|
||||
|
||||
#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
|
||||
#define KALMANFILTER_DEFAULT_FACTORIZATION QR
|
||||
|
|
@ -33,7 +32,13 @@ namespace gtsam {
|
|||
class SharedGaussian;
|
||||
|
||||
/**
|
||||
* Linear Kalman Filter
|
||||
* Kalman Filter class
|
||||
*
|
||||
* Knows how to maintain a Gaussian density under linear-Gaussian motion and
|
||||
* measurement models. It uses the square-root information form, as usual in GTSAM.
|
||||
*
|
||||
* The filter is functional, in that it does not have state: you call init() to create
|
||||
* an initial state, then predict() and update() that create new states out of old.
|
||||
*/
|
||||
class KalmanFilter {
|
||||
|
||||
|
|
@ -47,56 +52,42 @@ namespace gtsam {
|
|||
QR, LDL
|
||||
};
|
||||
|
||||
typedef GaussianDensity State;
|
||||
|
||||
private:
|
||||
|
||||
const size_t n_; /** dimensionality of state */
|
||||
const Matrix I_; /** identity matrix of size n*n */
|
||||
const Factorization method_; /** algorithm */
|
||||
|
||||
/// The Kalman filter posterior density is a Gaussian Conditional with no parents
|
||||
GaussianConditional::shared_ptr density_;
|
||||
|
||||
/// private constructor
|
||||
KalmanFilter(size_t n, const GaussianConditional::shared_ptr& density,
|
||||
Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION);
|
||||
|
||||
/// add a new factor and marginalize to new Kalman filter
|
||||
KalmanFilter add(GaussianFactor* newFactor);
|
||||
bool useQR() const { return method_==QR; }
|
||||
|
||||
public:
|
||||
|
||||
// private constructor
|
||||
KalmanFilter(size_t n, Factorization method =
|
||||
KALMANFILTER_DEFAULT_FACTORIZATION) :
|
||||
n_(n), I_(eye(n_, n_)), method_(method) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from prior density at time k=0
|
||||
* In Kalman Filter notation, these are is x_{0|0} and P_{0|0}
|
||||
* Create initial state, i.e., prior density at time k=0
|
||||
* In Kalman Filter notation, this are is x_{0|0} and P_{0|0}
|
||||
* @param x0 estimate at time 0
|
||||
* @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
|
||||
*/
|
||||
KalmanFilter(const Vector& x0, const SharedDiagonal& P0,
|
||||
Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION);
|
||||
State init(const Vector& x0, const SharedDiagonal& P0);
|
||||
|
||||
/**
|
||||
* Constructor from prior density at time k=0
|
||||
* In Kalman Filter notation, these are is x_{0|0} and P_{0|0}
|
||||
* @param x0 estimate at time 0
|
||||
* @param P0 covariance at time 0, full Gaussian
|
||||
*/
|
||||
KalmanFilter(const Vector& x0, const Matrix& P0, Factorization method =
|
||||
KALMANFILTER_DEFAULT_FACTORIZATION);
|
||||
/// version of init with a full covariance matrix
|
||||
State init(const Vector& x0, const Matrix& P0);
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/** Return step index k, starts at 0, incremented at each predict. */
|
||||
Index step() const { return density_->firstFrontalKey();}
|
||||
|
||||
/** Return mean of posterior P(x|Z) at given all measurements Z */
|
||||
Vector mean() const;
|
||||
|
||||
/** Return information matrix of posterior P(x|Z) at given all measurements Z */
|
||||
Matrix information() const;
|
||||
|
||||
/** Return covariance of posterior P(x|Z) at given all measurements Z */
|
||||
Matrix covariance() const;
|
||||
static Index step(const State& p) {
|
||||
return p.firstFrontalKey();
|
||||
}
|
||||
|
||||
/**
|
||||
* Predict the state P(x_{t+1}|Z^t)
|
||||
|
|
@ -107,8 +98,8 @@ namespace gtsam {
|
|||
* 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.
|
||||
*/
|
||||
KalmanFilter predict(const Matrix& F, const Matrix& B, const Vector& u,
|
||||
const SharedDiagonal& modelQ);
|
||||
KalmanFilter::State predict(const State& p, const Matrix& F,
|
||||
const Matrix& B, const Vector& u, const SharedDiagonal& modelQ);
|
||||
|
||||
/*
|
||||
* Version of predict with full covariance
|
||||
|
|
@ -116,8 +107,8 @@ namespace gtsam {
|
|||
* physical property, such as velocity or acceleration, and G is derived from physics.
|
||||
* This version allows more realistic models than a diagonal covariance matrix.
|
||||
*/
|
||||
KalmanFilter predictQ(const Matrix& F, const Matrix& B, const Vector& u,
|
||||
const Matrix& Q);
|
||||
KalmanFilter::State predictQ(const State& p, const Matrix& F,
|
||||
const Matrix& B, const Vector& u, const Matrix& Q);
|
||||
|
||||
/**
|
||||
* Predict the state P(x_{t+1}|Z^t)
|
||||
|
|
@ -127,8 +118,8 @@ namespace gtsam {
|
|||
* This version of predict takes GaussianFactor motion model [A0 A1 b]
|
||||
* with an optional noise model.
|
||||
*/
|
||||
KalmanFilter predict2(const Matrix& A0, const Matrix& A1, const Vector& b,
|
||||
const SharedDiagonal& model);
|
||||
KalmanFilter::State predict2(const State& p, const Matrix& A0,
|
||||
const Matrix& A1, const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/**
|
||||
* Update Kalman filter with a measurement
|
||||
|
|
@ -138,7 +129,7 @@ namespace gtsam {
|
|||
* Gaussian white noise with covariance R.
|
||||
* Currently, R is restricted to diagonal Gaussians (model parameter)
|
||||
*/
|
||||
KalmanFilter update(const Matrix& H, const Vector& z,
|
||||
KalmanFilter::State update(const State& p, const Matrix& H, const Vector& z,
|
||||
const SharedDiagonal& model);
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -37,42 +37,50 @@ struct State: Vector {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( KalmanFilter, constructor ) {
|
||||
// Create the Kalman Filter initialization point
|
||||
State x_initial(0.0,0.0);
|
||||
SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2,0.1);
|
||||
|
||||
// Create an KalmanFilter object
|
||||
KalmanFilter kf1(x_initial, P1);
|
||||
Matrix Sigma = Matrix_(2,2,0.01,0.0,0.0,0.01);
|
||||
EXPECT(assert_equal(Sigma,kf1.covariance()));
|
||||
// Create a Kalman filter of dimension 2
|
||||
KalmanFilter kf1(2);
|
||||
|
||||
// Create inital mean/covariance
|
||||
State x_initial(0.0, 0.0);
|
||||
SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2, 0.1);
|
||||
|
||||
// Get initial state by passing initial mean/covariance to the p
|
||||
KalmanFilter::State p1 = kf1.init(x_initial, P1);
|
||||
|
||||
// Assert it has the correct mean, covariance and information
|
||||
EXPECT(assert_equal(x_initial, p1.mean()));
|
||||
Matrix Sigma = Matrix_(2, 2, 0.01, 0.0, 0.0, 0.01);
|
||||
EXPECT(assert_equal(Sigma, p1.covariance()));
|
||||
EXPECT(assert_equal(inverse(Sigma), p1.information()));
|
||||
|
||||
// Create one with a sharedGaussian
|
||||
KalmanFilter kf2(x_initial, Sigma);
|
||||
EXPECT(assert_equal(Sigma,kf2.covariance()));
|
||||
KalmanFilter::State p2 = kf1.init(x_initial, Sigma);
|
||||
EXPECT(assert_equal(Sigma, p2.covariance()));
|
||||
|
||||
// Now make sure both agree
|
||||
EXPECT(assert_equal(kf1.covariance(),kf2.covariance()));
|
||||
EXPECT(assert_equal(p1.covariance(), p2.covariance()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( KalmanFilter, linear1 ) {
|
||||
|
||||
// Create the controls and measurement properties for our example
|
||||
Matrix F = eye(2,2);
|
||||
Matrix B = eye(2,2);
|
||||
Matrix F = eye(2, 2);
|
||||
Matrix B = eye(2, 2);
|
||||
Vector u = Vector_(2, 1.0, 0.0);
|
||||
SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
|
||||
Matrix Q = 0.01*eye(2,2);
|
||||
Matrix H = eye(2,2);
|
||||
Matrix Q = 0.01*eye(2, 2);
|
||||
Matrix H = eye(2, 2);
|
||||
State z1(1.0, 0.0);
|
||||
State z2(2.0, 0.0);
|
||||
State z3(3.0, 0.0);
|
||||
SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1);
|
||||
Matrix R = 0.01*eye(2,2);
|
||||
Matrix R = 0.01*eye(2, 2);
|
||||
|
||||
// Create the set of expected output TestValues
|
||||
State expected0(0.0, 0.0);
|
||||
Matrix P00 = 0.01*eye(2,2);
|
||||
Matrix P00 = 0.01*eye(2, 2);
|
||||
|
||||
State expected1(1.0, 0.0);
|
||||
Matrix P01 = P00 + Q;
|
||||
|
|
@ -86,68 +94,74 @@ TEST( KalmanFilter, linear1 ) {
|
|||
Matrix P23 = inverse(I22) + Q;
|
||||
Matrix I33 = inverse(P23) + inverse(R);
|
||||
|
||||
// Create a Kalman filter of dimension 2
|
||||
KalmanFilter kf(2);
|
||||
|
||||
// Create the Kalman Filter initialization point
|
||||
State x_initial(0.0,0.0);
|
||||
SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,0.1);
|
||||
State x_initial(0.0, 0.0);
|
||||
SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 0.1);
|
||||
|
||||
// Create initial KalmanFilter object
|
||||
KalmanFilter KF0(x_initial, P_initial);
|
||||
EXPECT(assert_equal(expected0,KF0.mean()));
|
||||
EXPECT(assert_equal(P00,KF0.covariance()));
|
||||
KalmanFilter::State p0 = kf.init(x_initial, P_initial);
|
||||
EXPECT(assert_equal(expected0, p0.mean()));
|
||||
EXPECT(assert_equal(P00, p0.covariance()));
|
||||
|
||||
// Run iteration 1
|
||||
KalmanFilter KF1p = KF0.predict(F, B, u, modelQ);
|
||||
EXPECT(assert_equal(expected1,KF1p.mean()));
|
||||
EXPECT(assert_equal(P01,KF1p.covariance()));
|
||||
KalmanFilter::State p1p = kf.predict(p0, F, B, u, modelQ);
|
||||
EXPECT(assert_equal(expected1, p1p.mean()));
|
||||
EXPECT(assert_equal(P01, p1p.covariance()));
|
||||
|
||||
KalmanFilter KF1 = KF1p.update(H,z1,modelR);
|
||||
EXPECT(assert_equal(expected1,KF1.mean()));
|
||||
EXPECT(assert_equal(I11,KF1.information()));
|
||||
KalmanFilter::State p1 = kf.update(p1p, H, z1, modelR);
|
||||
EXPECT(assert_equal(expected1, p1.mean()));
|
||||
EXPECT(assert_equal(I11, p1.information()));
|
||||
|
||||
// Run iteration 2 (with full covariance)
|
||||
KalmanFilter KF2p = KF1.predictQ(F, B, u, Q);
|
||||
EXPECT(assert_equal(expected2,KF2p.mean()));
|
||||
KalmanFilter::State p2p = kf.predictQ(p1, F, B, u, Q);
|
||||
EXPECT(assert_equal(expected2, p2p.mean()));
|
||||
|
||||
KalmanFilter KF2 = KF2p.update(H,z2,modelR);
|
||||
EXPECT(assert_equal(expected2,KF2.mean()));
|
||||
KalmanFilter::State p2 = kf.update(p2p, H, z2, modelR);
|
||||
EXPECT(assert_equal(expected2, p2.mean()));
|
||||
|
||||
// Run iteration 3
|
||||
KalmanFilter KF3p = KF2.predict(F, B, u, modelQ);
|
||||
EXPECT(assert_equal(expected3,KF3p.mean()));
|
||||
LONGS_EQUAL(3,KF3p.step());
|
||||
KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ);
|
||||
EXPECT(assert_equal(expected3, p3p.mean()));
|
||||
LONGS_EQUAL(3, KalmanFilter::step(p3p));
|
||||
|
||||
KalmanFilter KF3 = KF3p.update(H,z3,modelR);
|
||||
EXPECT(assert_equal(expected3,KF3.mean()));
|
||||
LONGS_EQUAL(3,KF3.step());
|
||||
KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR);
|
||||
EXPECT(assert_equal(expected3, p3.mean()));
|
||||
LONGS_EQUAL(3, KalmanFilter::step(p3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( KalmanFilter, predict ) {
|
||||
|
||||
// Create dynamics model
|
||||
Matrix F = Matrix_(2,2, 1.0,0.1, 0.2,1.1);
|
||||
Matrix B = Matrix_(2,3, 1.0,0.1,0.2, 1.1,1.2,0.8);
|
||||
Matrix F = Matrix_(2, 2, 1.0, 0.1, 0.2, 1.1);
|
||||
Matrix B = Matrix_(2, 3, 1.0, 0.1, 0.2, 1.1, 1.2, 0.8);
|
||||
Vector u = Vector_(3, 1.0, 0.0, 2.0);
|
||||
Matrix R = Matrix_(2,2, 1.0,0.5, 0.0,3.0);
|
||||
Matrix R = Matrix_(2, 2, 1.0, 0.5, 0.0, 3.0);
|
||||
Matrix M = trans(R)*R;
|
||||
Matrix Q = inverse(M);
|
||||
|
||||
// Create the Kalman Filter initialization point
|
||||
State x_initial(0.0,0.0);
|
||||
SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,1);
|
||||
// Create a Kalman filter of dimension 2
|
||||
KalmanFilter kf(2);
|
||||
|
||||
// Create two KalmanFilter objects
|
||||
KalmanFilter KF0(x_initial, P_initial);
|
||||
// Create the Kalman Filter initialization point
|
||||
State x_initial(0.0, 0.0);
|
||||
SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 1);
|
||||
|
||||
// Create initial KalmanFilter state
|
||||
KalmanFilter::State p0 = kf.init(x_initial, P_initial);
|
||||
|
||||
// Ensure predictQ and predict2 give same answer for non-trivial inputs
|
||||
KalmanFilter KFa = KF0.predictQ(F, B, u, Q);
|
||||
// We have A1 = -F, A2 = I_, b = B*u, pre-multipled with R to match Q noise model
|
||||
KalmanFilter::State pa = kf.predictQ(p0, F, B, u, Q);
|
||||
// We have A1 = -F, A2 = I_, b = B*u, pre-multipled with R to match Q noise model
|
||||
Matrix A1 = -R*F, A2 = R;
|
||||
Vector b = R*B*u;
|
||||
SharedDiagonal nop = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
KalmanFilter KFb = KF0.predict2(A1,A2,b,nop);
|
||||
EXPECT(assert_equal(KFa.mean(),KFb.mean()));
|
||||
EXPECT(assert_equal(KFa.covariance(),KFb.covariance()));
|
||||
KalmanFilter::State pb = kf.predict2(p0, A1, A2, b, nop);
|
||||
EXPECT(assert_equal(pa.mean(), pb.mean()));
|
||||
EXPECT(assert_equal(pa.covariance(), pb.covariance()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -155,7 +169,7 @@ TEST( KalmanFilter, predict ) {
|
|||
TEST( KalmanFilter, QRvsCholesky ) {
|
||||
|
||||
Vector mean = ones(9);
|
||||
Matrix covariance = 1e-6*Matrix_(9,9,
|
||||
Matrix covariance = 1e-6*Matrix_(9, 9,
|
||||
15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6,
|
||||
-6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1,
|
||||
0.0, -0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.1, -0.0,
|
||||
|
|
@ -166,8 +180,15 @@ TEST( KalmanFilter, QRvsCholesky ) {
|
|||
63.8, -0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0,
|
||||
-0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0);
|
||||
|
||||
// Create two Kalman filter of dimension 9, one using QR the other LDL
|
||||
KalmanFilter kfa(9, KalmanFilter::QR), kfb(9, KalmanFilter::LDL);
|
||||
|
||||
// create corresponding initial states
|
||||
KalmanFilter::State p0a = kfa.init(mean, covariance);
|
||||
KalmanFilter::State p0b = kfb.init(mean, covariance);
|
||||
|
||||
// Set up dynamics update
|
||||
Matrix Psi_k = 1e-6*Matrix_(9,9,
|
||||
Matrix Psi_k = 1e-6*Matrix_(9, 9,
|
||||
1000000.0, 0.0, 0.0, -19200.0, 600.0, -0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 1000000.0, 0.0, 600.0, 19200.0, 200.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1000000.0, -0.0, -200.0, 19200.0, 0.0, 0.0, 0.0,
|
||||
|
|
@ -177,9 +198,9 @@ TEST( KalmanFilter, QRvsCholesky ) {
|
|||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0);
|
||||
Matrix B = zeros(9,1);
|
||||
Matrix B = zeros(9, 1);
|
||||
Vector u = zero(1);
|
||||
Matrix dt_Q_k = 1e-6*Matrix_(9,9,
|
||||
Matrix dt_Q_k = 1e-6*Matrix_(9, 9,
|
||||
33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
-0.0, -0.3, 88.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
|
|
@ -190,19 +211,19 @@ TEST( KalmanFilter, QRvsCholesky ) {
|
|||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2);
|
||||
|
||||
// Create two KalmanFilter using different factorization method and compare
|
||||
KalmanFilter KFa = KalmanFilter(mean, covariance,KalmanFilter::QR).predictQ(Psi_k,B,u,dt_Q_k);
|
||||
KalmanFilter KFb = KalmanFilter(mean, covariance,KalmanFilter::LDL).predictQ(Psi_k,B,u,dt_Q_k);
|
||||
// Do prediction step
|
||||
KalmanFilter::State pa = kfa.predictQ(p0a, Psi_k, B, u, dt_Q_k);
|
||||
KalmanFilter::State pb = kfb.predictQ(p0b, Psi_k, B, u, dt_Q_k);
|
||||
|
||||
// Check that they yield the same mean and information matrix
|
||||
EXPECT(assert_equal(KFa.mean(),KFb.mean()));
|
||||
EXPECT(assert_equal(KFa.information(),KFb.information(),1e-7));
|
||||
EXPECT(assert_equal(pa.mean(), pb.mean()));
|
||||
EXPECT(assert_equal(pa.information(), pb.information(), 1e-7));
|
||||
|
||||
// and in addition attain the correct covariance
|
||||
Vector expectedMean = Vector_(9, 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.);
|
||||
EXPECT(assert_equal(expectedMean,KFa.mean(),1e-7));
|
||||
EXPECT(assert_equal(expectedMean,KFb.mean(),1e-7));
|
||||
Matrix expected = 1e-6*Matrix_(9,9,
|
||||
EXPECT(assert_equal(expectedMean, pa.mean(), 1e-7));
|
||||
EXPECT(assert_equal(expectedMean, pb.mean(), 1e-7));
|
||||
Matrix expected = 1e-6*Matrix_(9, 9,
|
||||
48.8, -3.1, -0.0, -0.4, -0.4, 0.0, 0.0, 63.8, -0.6,
|
||||
-3.1, 148.4, -0.3, 0.5, 1.7, 0.2, -63.8, 0.0, -0.1,
|
||||
-0.0, -0.3, 188.0, -0.0, 0.2, 1.2, 0.0, 0.1, 0.0,
|
||||
|
|
@ -212,26 +233,31 @@ TEST( KalmanFilter, QRvsCholesky ) {
|
|||
0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0, 0.0,
|
||||
63.8, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0,
|
||||
-0.6, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 647.2);
|
||||
EXPECT(assert_equal(expected,KFa.covariance(),1e-7));
|
||||
EXPECT(assert_equal(expected,KFb.covariance(),1e-7));
|
||||
EXPECT(assert_equal(expected, pa.covariance(), 1e-7));
|
||||
EXPECT(assert_equal(expected, pb.covariance(), 1e-7));
|
||||
|
||||
Matrix H = 1e-3*Matrix_(3,9,
|
||||
// prepare update
|
||||
Matrix H = 1e-3*Matrix_(3, 9,
|
||||
0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0,
|
||||
-9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0,
|
||||
-83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.);
|
||||
Vector z = Vector_(3,0.2599 , 1.3327 , 0.2007);
|
||||
|
||||
Vector sigmas = Vector_(3, 0.3323 ,0.2470 ,0.1904);
|
||||
Vector z = Vector_(3, 0.2599 , 1.3327 , 0.2007);
|
||||
Vector sigmas = Vector_(3, 0.3323 , 0.2470 , 0.1904);
|
||||
SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
KalmanFilter KFa2 = KFa.update(H, z, modelR);
|
||||
KalmanFilter KFb2 = KFb.update(H, z, modelR);
|
||||
EXPECT(assert_equal(KFa2.mean(),KFb2.mean()));
|
||||
EXPECT(assert_equal(KFa2.information(),KFb2.information(),1e-7));
|
||||
|
||||
// do update
|
||||
KalmanFilter::State pa2 = kfa.update(pa, H, z, modelR);
|
||||
KalmanFilter::State pb2 = kfb.update(pb, H, z, modelR);
|
||||
|
||||
// Check that they yield the same mean and information matrix
|
||||
EXPECT(assert_equal(pa2.mean(), pb2.mean()));
|
||||
EXPECT(assert_equal(pa2.information(), pb2.information(), 1e-7));
|
||||
|
||||
// and in addition attain the correct mean and covariance
|
||||
Vector expectedMean2 = Vector_(9, 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882);
|
||||
EXPECT(assert_equal(expectedMean2,KFa2.mean(),1e-4)); // not happy with tolerance here !
|
||||
EXPECT(assert_equal(expectedMean2,KFb2.mean(),1e-4)); // is something still amiss?
|
||||
Matrix expected2 = 1e-6*Matrix_(9,9,
|
||||
EXPECT(assert_equal(expectedMean2, pa2.mean(), 1e-4));// not happy with tolerance here !
|
||||
EXPECT(assert_equal(expectedMean2, pb2.mean(), 1e-4));// is something still amiss?
|
||||
Matrix expected2 = 1e-6*Matrix_(9, 9,
|
||||
46.1, -2.6, -0.0, -0.4, -0.4, 0.0, 0.0, 63.9, -0.5,
|
||||
-2.6, 132.8, -0.5, 0.4, 1.5, 0.2, -64.0, -0.0, -0.1,
|
||||
-0.0, -0.5, 188.0, -0.0, 0.2, 1.2, -0.0, 0.1, 0.0,
|
||||
|
|
@ -241,8 +267,8 @@ TEST( KalmanFilter, QRvsCholesky ) {
|
|||
0.0, -64.0, -0.0, -0.0, -0.0, -0.0, 647.2, -0.0, 0.0,
|
||||
63.9, -0.0, 0.1, -0.0, -0.0, 0.0, -0.0, 647.2, 0.1,
|
||||
-0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8);
|
||||
EXPECT(assert_equal(expected2,KFa2.covariance(),1e-7));
|
||||
EXPECT(assert_equal(expected2,KFb2.covariance(),1e-7));
|
||||
EXPECT(assert_equal(expected2, pa2.covariance(), 1e-7));
|
||||
EXPECT(assert_equal(expected2, pb2.covariance(), 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue