Additional Kalman filter constructor takes covariance
parent
56c3953305
commit
a209741e60
144
.cproject
144
.cproject
|
@ -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
29
gtsam.h
|
@ -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 {
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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 ) {
|
||||
|
||||
|
|
Loading…
Reference in New Issue