12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wrapped new KF style and fixed test

release/4.3a0
Frank Dellaert 2012-01-27 22:20:43 +00:00
parent f260af51c3
commit ba34a43b1c
2 changed files with 42 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam.h
View File

@ -256,6 +256,14 @@ class GaussianConditional {
bool equals(const GaussianConditional &cg, double tol) const;
};
class GaussianDensity {
GaussianDensity(int key, Vector d, Matrix R, Vector sigmas);
void print(string s) const;
Vector mean() const;
Matrix information() const;
Matrix covariance() const;
};
class GaussianBayesNet {
GaussianBayesNet();
void print(string s) const;
@ -347,16 +355,19 @@ class GaussianSequentialSolver {
};
class KalmanFilter {
KalmanFilter(Vector x0, const SharedDiagonal& P0);
KalmanFilter(Vector x0, Matrix P0);
KalmanFilter(size_t n);
GaussianDensity* init(Vector x0, const SharedDiagonal& P0);
GaussianDensity* init(Vector x0, Matrix P0);
void print(string s) const;
Vector mean() const;
Matrix information() const;
Matrix covariance() const;
KalmanFilter predict(Matrix F, Matrix B, Vector u, const SharedDiagonal& model);
KalmanFilter predictQ(Matrix F, Matrix B, Vector u, Matrix Q);
KalmanFilter predict2(Matrix A0, Matrix A1, Vector b, const SharedDiagonal& model);
KalmanFilter update(Matrix H, Vector z, const SharedDiagonal& model);
static int step(GaussianDensity* p);
GaussianDensity* predict(GaussianDensity* p, Matrix F, Matrix B, Vector u,
const SharedDiagonal& modelQ);
GaussianDensity* predictQ(GaussianDensity* p, Matrix F, Matrix B, Vector u,
Matrix Q);
GaussianDensity* predict2(GaussianDensity* p, Matrix A0, Matrix A1, Vector b,
const SharedDiagonal& model);
GaussianDensity* update(GaussianDensity* p, Matrix H, Vector z,
const SharedDiagonal& model);
};
//*************************************************************************

41
tests/matlab/testKalmanFilter.m
View File

@ -47,31 +47,34 @@ expected3 = [3.0, 0.0]';
P23 = inv(I22) + Q;
I33 = inv(P23) + inv(R);
%% Create an KalmanFilter object
KF = KalmanFilter(2);
%% Create the Kalman Filter initialization point
x_initial = [0.0;0.0];
P_initial = SharedDiagonal([0.1;0.1]);
P_initial = 0.01*eye(2);
%% Create an KalmanFilter object
kalmanFilter = KalmanFilter(x_initial, P_initial)
EQUALITY('expected0,kalmanFilter.mean', expected0,kalmanFilter.mean);
EQUALITY('expected0,kalmanFilter.mean', P00,kalmanFilter.covariance);
%% Create an KF object
state = KF.init(x_initial, P_initial);
EQUALITY('expected0,state.mean', expected0,state.mean);
EQUALITY('expected0,state.mean', P00,state.covariance);
%% Run iteration 1
kalmanFilter.predict(F, B, u, modelQ);
EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean);
EQUALITY('P01,kalmanFilter.covariance', P01,kalmanFilter.covariance);
kalmanFilter.update(H,z1,modelR);
EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean);
EQUALITY('I11,kalmanFilter.information', I11,kalmanFilter.information);
state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected1,state.mean', expected1,state.mean);
EQUALITY('P01,state.covariance', P01,state.covariance);
state = KF.update(state,H,z1,modelR);
EQUALITY('expected1,state.mean', expected1,state.mean);
EQUALITY('I11,state.information', I11,state.information);
%% Run iteration 2
kalmanFilter.predict(F, B, u, modelQ);
EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean);
kalmanFilter.update(H,z2,modelR);
EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean);
state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected2,state.mean', expected2,state.mean);
state = KF.update(state,H,z2,modelR);
EQUALITY('expected2,state.mean', expected2,state.mean);
%% Run iteration 3
kalmanFilter.predict(F, B, u, modelQ);
EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean);
kalmanFilter.update(H,z3,modelR);
EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean);
state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected3,state.mean', expected3,state.mean);
state = KF.update(state,H,z3,modelR);
EQUALITY('expected3,state.mean', expected3,state.mean);