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

Additional Kalman filter constructor takes covariance

release/4.3a0
Frank Dellaert 2012-01-17 20:04:37 +00:00
parent 56c3953305
commit a209741e60
5 changed files with 101 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
.cproject
View File

@ -105,16 +105,6 @@
<storageModule moduleId="refreshScope"/>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
@ -125,36 +115,6 @@
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
@ -187,16 +147,6 @@
</profile>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544;cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194;cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.1629258328;cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
@ -207,36 +157,6 @@
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
@ -270,16 +190,6 @@
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544;cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194;cdt.managedbuild.tool.gnu.c.compiler.macosx.base.1347102680;cdt.managedbuild.tool.gnu.c.compiler.input.1718446861">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
@ -290,36 +200,6 @@
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
@ -1052,6 +932,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExtendedKalmanFilter.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testExtendedKalmanFilter.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1904,6 +1792,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testKalmanFilter.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2183,6 +2079,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="doxygen-doc" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>doxygen-doc</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

29
gtsam.h
View File

@ -57,6 +57,14 @@
// Everything is in the gtsam namespace, so we avoid copying everything in
using namespace gtsam;
//*************************************************************************
// base
//*************************************************************************
//*************************************************************************
// geometry
//*************************************************************************
class Point2 {
Point2();
Point2(double x, double y);
@ -197,6 +205,16 @@ class Pose3 {
Rot3 rotation() const;
};
//*************************************************************************
// inference
//*************************************************************************
//*************************************************************************
// linear
//*************************************************************************
class SharedGaussian {
SharedGaussian(Matrix covariance);
void print(string s) const;
@ -292,7 +310,8 @@ class GaussianSequentialSolver {
};
class KalmanFilter {
KalmanFilter(Vector x, const SharedDiagonal& model);
KalmanFilter(Vector x0, const SharedDiagonal& P0);
KalmanFilter(Vector x0, const Matrix& P0);
void print(string s) const;
Vector mean() const;
Matrix information() const;
@ -302,6 +321,10 @@ class KalmanFilter {
void update(Matrix H, Vector z, const SharedDiagonal& model);
};
//*************************************************************************
// nonlinear
//*************************************************************************
class Ordering {
Ordering();
void print(string s) const;
@ -309,6 +332,10 @@ class Ordering {
void push_back(string key);
};
//*************************************************************************
// slam
//*************************************************************************
// Planar SLAM example domain
#include <gtsam/slam/planarSLAM.h>
namespace planarSLAM {

18
gtsam/linear/KalmanFilter.cpp
View File

@ -23,6 +23,8 @@
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/HessianFactor.h>
namespace gtsam {
@ -42,12 +44,24 @@ namespace gtsam {
}
/* ************************************************************************* */
KalmanFilter::KalmanFilter(const Vector& x, const SharedDiagonal& model) :
KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0) :
n_(x0.size()), I_(eye(n_, n_)) {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma
density_.reset(solve(factorGraph));
}
/* ************************************************************************* */
KalmanFilter::KalmanFilter(const Vector& x, const Matrix& P0) :
n_(x.size()), I_(eye(n_, n_)) {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph.add(0, I_, x, model);
// 0.5*(x-x0)'*inv(Sigma)*(x-x0)
HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0));
factorGraph.push_back(factor);
density_.reset(solve(factorGraph));
}

15
gtsam/linear/KalmanFilter.h
View File

@ -25,6 +25,7 @@
namespace gtsam {
class SharedDiagonal;
class SharedGaussian;
/**
* Linear Kalman Filter
@ -43,10 +44,18 @@ namespace gtsam {
/**
* Constructor from prior density at time k=0
* In Kalman Filter notation, these are is x_{0|0} and P_{0|0}
* @param x estimate at time 0
* @param P covariance at time 0, restricted to diagonal Gaussian 'model' for now
* @param x0 estimate at time 0
* @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
*/
KalmanFilter(const Vector& x, const SharedDiagonal& model);
KalmanFilter(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);
/// print
void print(const std::string& s="") const {

21
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -20,6 +20,8 @@
#include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -34,6 +36,25 @@ 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 one with a sharedGaussian
KalmanFilter kf2(x_initial, Sigma);
EXPECT(assert_equal(Sigma,kf2.covariance()));
// Now make sure both agree
EXPECT(assert_equal(kf1.covariance(),kf2.covariance()));
}
/* ************************************************************************* */
TEST( KalmanFilter, linear1 ) {