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="refreshScope"/>
<storageModule moduleId="scannerConfiguration"> <storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/> <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"> <profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider> <buildOutputProvider>
<openAction enabled="true" filePath=""/> <openAction enabled="true" filePath=""/>
@ -125,36 +115,6 @@
<parser enabled="true"/> <parser enabled="true"/>
</scannerInfoProvider> </scannerInfoProvider>
</profile> </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"> <profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider> <buildOutputProvider>
<openAction enabled="true" filePath=""/> <openAction enabled="true" filePath=""/>
@ -187,16 +147,6 @@
</profile> </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"> <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"/> <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"> <profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider> <buildOutputProvider>
<openAction enabled="true" filePath=""/> <openAction enabled="true" filePath=""/>
@ -207,36 +157,6 @@
<parser enabled="true"/> <parser enabled="true"/>
</scannerInfoProvider> </scannerInfoProvider>
</profile> </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"> <profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider> <buildOutputProvider>
<openAction enabled="true" filePath=""/> <openAction enabled="true" filePath=""/>
@ -270,16 +190,6 @@
</scannerConfigBuildInfo> </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"> <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"/> <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"> <profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider> <buildOutputProvider>
<openAction enabled="true" filePath=""/> <openAction enabled="true" filePath=""/>
@ -290,36 +200,6 @@
<parser enabled="true"/> <parser enabled="true"/>
</scannerInfoProvider> </scannerInfoProvider>
</profile> </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"> <profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider> <buildOutputProvider>
<openAction enabled="true" filePath=""/> <openAction enabled="true" filePath=""/>
@ -1052,6 +932,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="SimpleRotation.run" path="examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1904,6 +1792,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2183,6 +2079,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check j5" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

29
gtsam.h
View File

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

18
gtsam/linear/KalmanFilter.cpp
View File

@ -23,6 +23,8 @@
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/HessianFactor.h>
namespace gtsam { 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_)) { n_(x.size()), I_(eye(n_, n_)) {
// Create a factor graph f(x0), eliminate it into P(x0) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph; 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)); density_.reset(solve(factorGraph));
} }

15
gtsam/linear/KalmanFilter.h
View File

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