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; 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 { class GaussianBayesNet {
GaussianBayesNet(); GaussianBayesNet();
void print(string s) const; void print(string s) const;
@ -347,16 +355,19 @@ class GaussianSequentialSolver {
}; };
class KalmanFilter { class KalmanFilter {
KalmanFilter(Vector x0, const SharedDiagonal& P0); KalmanFilter(size_t n);
KalmanFilter(Vector x0, Matrix P0); GaussianDensity* init(Vector x0, const SharedDiagonal& P0);
GaussianDensity* init(Vector x0, Matrix P0);
void print(string s) const; void print(string s) const;
Vector mean() const; static int step(GaussianDensity* p);
Matrix information() const; GaussianDensity* predict(GaussianDensity* p, Matrix F, Matrix B, Vector u,
Matrix covariance() const; const SharedDiagonal& modelQ);
KalmanFilter predict(Matrix F, Matrix B, Vector u, const SharedDiagonal& model); GaussianDensity* predictQ(GaussianDensity* p, Matrix F, Matrix B, Vector u,
KalmanFilter predictQ(Matrix F, Matrix B, Vector u, Matrix Q); Matrix Q);
KalmanFilter predict2(Matrix A0, Matrix A1, Vector b, const SharedDiagonal& model); GaussianDensity* predict2(GaussianDensity* p, Matrix A0, Matrix A1, Vector b,
KalmanFilter update(Matrix H, Vector z, const SharedDiagonal& model); 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; P23 = inv(I22) + Q;
I33 = inv(P23) + inv(R); I33 = inv(P23) + inv(R);
%% Create an KalmanFilter object
KF = KalmanFilter(2);
%% Create the Kalman Filter initialization point %% Create the Kalman Filter initialization point
x_initial = [0.0;0.0]; x_initial = [0.0;0.0];
P_initial = SharedDiagonal([0.1;0.1]); P_initial = 0.01*eye(2);
%% Create an KalmanFilter object %% Create an KF object
kalmanFilter = KalmanFilter(x_initial, P_initial) state = KF.init(x_initial, P_initial);
EQUALITY('expected0,kalmanFilter.mean', expected0,kalmanFilter.mean); EQUALITY('expected0,state.mean', expected0,state.mean);
EQUALITY('expected0,kalmanFilter.mean', P00,kalmanFilter.covariance); EQUALITY('expected0,state.mean', P00,state.covariance);
%% Run iteration 1 %% Run iteration 1
kalmanFilter.predict(F, B, u, modelQ); state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean); EQUALITY('expected1,state.mean', expected1,state.mean);
EQUALITY('P01,kalmanFilter.covariance', P01,kalmanFilter.covariance); EQUALITY('P01,state.covariance', P01,state.covariance);
kalmanFilter.update(H,z1,modelR); state = KF.update(state,H,z1,modelR);
EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean); EQUALITY('expected1,state.mean', expected1,state.mean);
EQUALITY('I11,kalmanFilter.information', I11,kalmanFilter.information); EQUALITY('I11,state.information', I11,state.information);
%% Run iteration 2 %% Run iteration 2
kalmanFilter.predict(F, B, u, modelQ); state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean); EQUALITY('expected2,state.mean', expected2,state.mean);
kalmanFilter.update(H,z2,modelR); state = KF.update(state,H,z2,modelR);
EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean); EQUALITY('expected2,state.mean', expected2,state.mean);
%% Run iteration 3 %% Run iteration 3
kalmanFilter.predict(F, B, u, modelQ); state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean); EQUALITY('expected3,state.mean', expected3,state.mean);
kalmanFilter.update(H,z3,modelR); state = KF.update(state,H,z3,modelR);
EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean); EQUALITY('expected3,state.mean', expected3,state.mean);