Moved AHRS to gtsam_unstable
Frank Dellaert
parent
314b854ecb
commit
1d9000724d
362
.cproject
362
.cproject
|
|
@ -1,5 +1,7 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
|
||||
|
|
@ -307,14 +309,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -341,6 +335,7 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -348,6 +343,7 @@
|
|||
</target>
|
||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -395,6 +391,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -402,6 +399,7 @@
|
|||
</target>
|
||||
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -409,6 +407,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -424,11 +423,20 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -517,22 +525,6 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -549,6 +541,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -573,34 +581,26 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testValues.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testKey.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLinearContainerFactor.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -685,26 +685,34 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testValues.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testKey.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLinearContainerFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -821,6 +829,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testAHRS.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testAHRS.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1055,6 +1071,7 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1062,6 +1079,7 @@
|
|||
</target>
|
||||
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testJunctionTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1069,6 +1087,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1236,6 +1255,7 @@
|
|||
</target>
|
||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testErrors.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1281,14 +1301,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1369,6 +1381,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1675,7 +1695,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1715,7 +1734,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1723,7 +1741,6 @@
|
|||
</target>
|
||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated3D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1923,7 +1940,6 @@
|
|||
</target>
|
||||
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1945,102 +1961,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>timeRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCal3_S2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testHomography2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j3</buildArguments>
|
||||
|
|
@ -2242,6 +2162,7 @@
|
|||
</target>
|
||||
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G DEB</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2249,6 +2170,7 @@
|
|||
</target>
|
||||
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G RPM</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2256,6 +2178,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G TGZ</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2263,6 +2186,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2404,34 +2328,98 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSpirit.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testWrap.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.wrap</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>timeRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCal3_S2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testHomography2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2475,6 +2463,38 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSpirit.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testWrap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cproject>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,244 @@
|
|||
/*
|
||||
* @file AHRS.cpp
|
||||
* @brief Attitude and Heading Reference System implementation
|
||||
* Created on: Jan 26, 2012
|
||||
* Author: cbeall3
|
||||
*/
|
||||
|
||||
#include "AHRS.h"
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
Matrix cov(const Matrix& m) {
|
||||
const double num_observations = m.cols();
|
||||
const Vector mean = m.rowwise().sum() / num_observations;
|
||||
Matrix D = m.colwise() - mean;
|
||||
Matrix DDt = D * trans(D);
|
||||
return DDt / (num_observations - 1);
|
||||
}
|
||||
|
||||
Matrix I3 = eye(3);
|
||||
Matrix Z3 = zeros(3, 3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e) :
|
||||
KF_(9) {
|
||||
|
||||
// Initial state
|
||||
mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e);
|
||||
|
||||
size_t T = stationaryU.cols();
|
||||
|
||||
// estimate standard deviation on gyroscope readings
|
||||
Pg_ = cov(stationaryU);
|
||||
Vector sigmas_v_g = esqrt(Pg_.diagonal() * T);
|
||||
|
||||
// estimate standard deviation on accelerometer readings
|
||||
Pa_ = cov(stationaryF);
|
||||
|
||||
// Quantities needed for integration
|
||||
|
||||
// dynamics, Chris' email September 23, 2011 3:38:05 PM EDT
|
||||
double tau_g = 730; // correlation time for gyroscope
|
||||
double tau_a = 730; // correlation time for accelerometer
|
||||
|
||||
F_g_ = -I3 / tau_g;
|
||||
F_a_ = -I3 / tau_a;
|
||||
Vector var_omega_w = 0 * ones(3); // TODO
|
||||
Vector var_omega_g = (0.0034 * 0.0034) * ones(3);
|
||||
Vector var_omega_a = (0.034 * 0.034) * ones(3);
|
||||
Vector sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
|
||||
Vector vars = concatVectors(4, &var_omega_w, &var_omega_g, &sigmas_v_g_sq,
|
||||
&var_omega_a);
|
||||
var_w_ = diag(vars);
|
||||
|
||||
// Quantities needed for aiding
|
||||
sigmas_v_a_ = esqrt(T * Pa_.diagonal());
|
||||
|
||||
// gravity in nav frame
|
||||
n_g_ = Vector_(3, 0.0, 0.0, g_e);
|
||||
n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!!
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e) {
|
||||
|
||||
// Calculate Omega_T, formula 2.80 in Farrell08book
|
||||
double cp = cos(mech0_.bRn().inverse().pitch());
|
||||
double sp = sin(mech0_.bRn().inverse().pitch());
|
||||
double cy = cos(0);
|
||||
double sy = sin(0);
|
||||
Matrix Omega_T = Matrix_(3, 3, cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0);
|
||||
|
||||
// Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb
|
||||
Vector b_g = mech0_.b_g(g_e);
|
||||
double g1 = b_g(0);
|
||||
double g2 = b_g(1);
|
||||
double g3 = b_g(2);
|
||||
double g23 = g2 * g2 + g3 * g3;
|
||||
double g123 = g1 * g1 + g23;
|
||||
double f = 1 / (sqrt(g23) * g123);
|
||||
Matrix H_g = Matrix_(3, 3,
|
||||
0.0, g3 / g23, -(g2 / g23), // roll
|
||||
sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch
|
||||
0.0, 0.0, 0.0); // we don't know anything on yaw
|
||||
|
||||
// Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66
|
||||
Matrix Pa = 0.025 * 0.025 * eye(3);
|
||||
Matrix P11 = Omega_T * (H_g * (Pa + Pa_) * trans(H_g)) * trans(Omega_T);
|
||||
P11(2, 2) = 0.0001;
|
||||
Matrix P12 = -Omega_T * H_g * Pa;
|
||||
|
||||
Matrix P_plus_k2 = Matrix(9, 9);
|
||||
P_plus_k2.block(0, 0, 3, 3) = P11;
|
||||
P_plus_k2.block(0, 3, 3, 3) = Z3;
|
||||
P_plus_k2.block(0, 6, 3, 3) = P12;
|
||||
|
||||
P_plus_k2.block(3, 0, 3, 3) = Z3;
|
||||
P_plus_k2.block(3, 3, 3, 3) = Pg_;
|
||||
P_plus_k2.block(3, 6, 3, 3) = Z3;
|
||||
|
||||
P_plus_k2.block(6, 0, 3, 3) = trans(P12);
|
||||
P_plus_k2.block(6, 3, 3, 3) = Z3;
|
||||
P_plus_k2.block(6, 6, 3, 3) = Pa;
|
||||
|
||||
Vector dx = zero(9);
|
||||
KalmanFilter::State state = KF_.init(dx, P_plus_k2);
|
||||
return std::make_pair(mech0_, state);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
|
||||
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
||||
const Vector& u, double dt) {
|
||||
|
||||
// Integrate full state
|
||||
Mechanization_bRn2 newMech = mech.integrate(u, dt);
|
||||
|
||||
// Integrate error state Kalman filter
|
||||
// FIXME:
|
||||
//if nargout>1
|
||||
Matrix bRn = mech.bRn().matrix();
|
||||
Matrix I3 = eye(3);
|
||||
Matrix Z3 = zeros(3, 3);
|
||||
|
||||
Matrix F_k = zeros(9, 9);
|
||||
F_k.block(0, 3, 3, 3) = -bRn;
|
||||
F_k.block(3, 3, 3, 3) = F_g_;
|
||||
F_k.block(6, 6, 3, 3) = F_a_;
|
||||
|
||||
Matrix G_k = zeros(9, 12);
|
||||
G_k.block(0, 0, 3, 3) = -bRn;
|
||||
G_k.block(0, 6, 3, 3) = -bRn;
|
||||
G_k.block(3, 3, 3, 3) = I3;
|
||||
G_k.block(6, 9, 3, 3) = I3;
|
||||
|
||||
Matrix Q_k = G_k * var_w_ * trans(G_k);
|
||||
Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
|
||||
|
||||
Matrix B = zeros(9, 9);
|
||||
Vector u2 = zero(9);
|
||||
KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k);
|
||||
return make_pair(newMech, newState);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
|
||||
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
||||
const Vector& f, bool Farrell) {
|
||||
|
||||
Matrix bRn = mech.bRn().matrix();
|
||||
|
||||
// get gravity in body from accelerometer
|
||||
Vector measured_b_g = mech.x_a() - f;
|
||||
|
||||
Matrix R, H;
|
||||
Vector z;
|
||||
if (Farrell) {
|
||||
// calculate residual gravity measurement
|
||||
z = n_g_ - trans(bRn) * measured_b_g;
|
||||
H = collect(3, &n_g_cross_, &Z3, &bRn);
|
||||
R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn;
|
||||
} else {
|
||||
// my measurement prediction (in body frame):
|
||||
// F(:,k) = bias - b_g
|
||||
// F(:,k) = mech.x_a + dx_a - bRn*n_g;
|
||||
// F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g;
|
||||
// F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x
|
||||
// b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho);
|
||||
z = bRn * n_g_ - measured_b_g;
|
||||
Matrix b_g = bRn * n_g_cross_;
|
||||
H = collect(3, &b_g, &Z3, &I3);
|
||||
R = diag(emul(sigmas_v_a_, sigmas_v_a_));
|
||||
}
|
||||
|
||||
// update the Kalman filter
|
||||
KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R);
|
||||
|
||||
// update full state (rotation and accelerometer bias)
|
||||
Mechanization_bRn2 newMech = mech.correct(updatedState->mean());
|
||||
|
||||
// reset KF state
|
||||
Vector dx = zeros(9, 1);
|
||||
updatedState = KF_.init(dx, updatedState->covariance());
|
||||
|
||||
return make_pair(newMech, updatedState);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
|
||||
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
||||
const Vector& f, const Vector& f_previous,
|
||||
const Rot3& R_previous) {
|
||||
|
||||
Matrix increment = R_previous.between(mech.bRn()).matrix();
|
||||
|
||||
// expected - measured
|
||||
Vector z = f - increment * f_previous;
|
||||
//Vector z = increment * f_previous - f;
|
||||
Matrix b_g = skewSymmetric(increment* f_previous);
|
||||
Matrix H = collect(3, &b_g, &I3, &Z3);
|
||||
// Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
|
||||
// Matrix R = diag(Vector_(3, 1.0, 0.2, 1.0)); // good for L_twice
|
||||
Matrix R = diag(Vector_(3, 0.01, 0.0001, 0.01));
|
||||
|
||||
// update the Kalman filter
|
||||
KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R);
|
||||
|
||||
// update full state (rotation and gyro bias)
|
||||
Mechanization_bRn2 newMech = mech.correct(updatedState->mean());
|
||||
|
||||
// reset KF state
|
||||
Vector dx = zeros(9, 1);
|
||||
updatedState = KF_.init(dx, updatedState->covariance());
|
||||
|
||||
return make_pair(newMech, updatedState);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void AHRS::print(const std::string& s) const {
|
||||
mech0_.print(s + ".mech0_");
|
||||
gtsam::print(F_g_, s + ".F_g");
|
||||
gtsam::print(F_a_, s + ".F_a");
|
||||
gtsam::print(var_w_, s + ".var_w");
|
||||
|
||||
gtsam::print(sigmas_v_a_, s + ".sigmas_v_a");
|
||||
gtsam::print(n_g_, s + ".n_g");
|
||||
gtsam::print(n_g_cross_, s + ".n_g_cross");
|
||||
|
||||
gtsam::print(Pg_, s + ".P_g");
|
||||
gtsam::print(Pa_, s + ".P_a");
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
AHRS::~AHRS() {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/*
|
||||
* AHRS.h
|
||||
*
|
||||
* Created on: Jan 26, 2012
|
||||
* Author: cbeall3
|
||||
*/
|
||||
|
||||
#ifndef AHRS_H_
|
||||
#define AHRS_H_
|
||||
|
||||
#include "Mechanization_bRn2.h"
|
||||
#include <gtsam/linear/KalmanFilter.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
Matrix cov(const Matrix& m);
|
||||
|
||||
class AHRS {
|
||||
|
||||
private:
|
||||
|
||||
// Initial state
|
||||
Mechanization_bRn2 mech0_; ///< Initial mechanization state
|
||||
KalmanFilter KF_; ///< initial Kalman filter
|
||||
|
||||
// Quantities needed for integration
|
||||
Matrix F_g_; ///< gyro bias dynamics
|
||||
Matrix F_a_; ///< acc bias dynamics
|
||||
Matrix var_w_; ///< dynamic noise variances
|
||||
|
||||
// Quantities needed for aiding
|
||||
Vector sigmas_v_a_; ///< measurement sigma
|
||||
Vector n_g_; ///< gravity in nav frame
|
||||
Matrix n_g_cross_; ///< and its skew-symmetric matrix
|
||||
|
||||
Matrix Pg_, Pa_;
|
||||
|
||||
public:
|
||||
/**
|
||||
* AHRS constructor
|
||||
* @param stationaryU initial interval of gyro measurements, 3xn matrix
|
||||
* @param stationaryF initial interval of accelerator measurements, 3xn matrix
|
||||
* @param g_e if given, initializes gravity with correct value g_e
|
||||
*/
|
||||
AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e);
|
||||
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> initialize(double g_e);
|
||||
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> integrate(
|
||||
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
||||
const Vector& u, double dt);
|
||||
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> aid(
|
||||
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
||||
const Vector& f, bool Farrell=0);
|
||||
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> aidGeneral(
|
||||
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
||||
const Vector& f, const Vector& f_expected, const Rot3& R_previous);
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
virtual ~AHRS();
|
||||
};
|
||||
|
||||
} /* namespace gtsam */
|
||||
#endif /* AHRS_H_ */
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
/**
|
||||
* @file Mechanization_bRn.cpp
|
||||
* @date Jan 25, 2012
|
||||
* @author Chris Beall
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include "Mechanization_bRn2.h"
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list<Vector>& U,
|
||||
const std::list<Vector>& F, const double g_e) {
|
||||
Matrix Umat = Matrix_(3,U.size(), concatVectors(U));
|
||||
Matrix Fmat = Matrix_(3,F.size(), concatVectors(F));
|
||||
|
||||
return initialize(Umat, Fmat, g_e);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
|
||||
const Matrix& F, const double g_e) {
|
||||
|
||||
// estimate gyro bias by averaging gyroscope readings (10.62)
|
||||
Vector x_g = U.rowwise().mean();
|
||||
Vector meanF = -F.rowwise().mean();
|
||||
|
||||
Vector b_g, x_a;
|
||||
if(g_e == 0) {
|
||||
// estimate gravity in body frame from accelerometer (10.13)
|
||||
b_g = meanF;
|
||||
// estimate accelerometer bias as zero (10.65)
|
||||
x_a = zero(3);
|
||||
|
||||
} else {
|
||||
// normalize b_g and attribute remainder to biases
|
||||
b_g = g_e * meanF/meanF.norm();
|
||||
x_a = -(meanF - b_g);
|
||||
}
|
||||
|
||||
// initialize roll, pitch (eqns. 10.14, 10.15)
|
||||
double g1=b_g(0);
|
||||
double g2=b_g(1);
|
||||
double g3=b_g(2);
|
||||
|
||||
// Farrell08book eqn. 10.14
|
||||
double roll = atan2(g2, g3);
|
||||
// Farrell08book eqn. 10.15
|
||||
double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3));
|
||||
double yaw = 0;
|
||||
// This returns body-to-nav nRb
|
||||
Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse();
|
||||
|
||||
return Mechanization_bRn2(bRn, x_g, x_a);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Mechanization_bRn2 Mechanization_bRn2::correct(const Vector& dx) const {
|
||||
Vector rho = sub(dx, 0, 3);
|
||||
|
||||
Rot3 delta_nRn = Rot3::rodriguez(rho);
|
||||
Rot3 bRn = bRn_ * delta_nRn;
|
||||
|
||||
Vector x_g = x_g_ + sub(dx, 3, 6);
|
||||
Vector x_a = x_a_ + sub(dx, 6, 9);
|
||||
|
||||
return Mechanization_bRn2(bRn, x_g, x_a);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u,
|
||||
const double dt) const {
|
||||
// integrate rotation nRb based on gyro measurement u and bias x_g
|
||||
|
||||
// get body to inertial (measured in b) from gyro, subtract bias
|
||||
Vector b_omega_ib = u - x_g_;
|
||||
|
||||
// assume nav to inertial through movement is unknown
|
||||
Vector b_omega_in = zero(3);
|
||||
|
||||
// get angular velocity on bRn measured in body frame
|
||||
Vector b_omega_bn = b_omega_in - b_omega_ib;
|
||||
|
||||
// convert to navigation frame
|
||||
Rot3 nRb = bRn_.inverse();
|
||||
Vector n_omega_bn = (nRb*b_omega_bn).vector();
|
||||
|
||||
// integrate bRn using exponential map, assuming constant over dt
|
||||
Rot3 delta_nRn = Rot3::rodriguez(n_omega_bn*dt);
|
||||
Rot3 bRn = bRn_.compose(delta_nRn);
|
||||
|
||||
// implicit updating of biases (variables just do not change)
|
||||
// x_g = x_g;
|
||||
// x_a = x_a;
|
||||
return Mechanization_bRn2(bRn, x_g_, x_a_);
|
||||
}
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/**
|
||||
* @file Mechanization_bRn.h
|
||||
* @date Jan 25, 2012
|
||||
* @author Chris Beall
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <list>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Mechanization_bRn2 {
|
||||
|
||||
private:
|
||||
Rot3 bRn_; ///< rotation from nav to body
|
||||
Vector x_g_; ///< gyroscope bias
|
||||
Vector x_a_; ///< accelerometer bias
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param initial_bRn initial rotation from nav to body frame
|
||||
* @param initial_x_g initial gyro bias
|
||||
* @param r3 Z-axis of rotated frame
|
||||
*/
|
||||
Mechanization_bRn2(const Rot3& initial_bRn = Rot3(),
|
||||
const Vector& initial_x_g = zero(3), const Vector& initial_x_a = zero(3)) :
|
||||
bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) {
|
||||
}
|
||||
|
||||
Vector b_g(double g_e) {
|
||||
Vector n_g = Vector_(3, 0.0, 0.0, g_e);
|
||||
return (bRn_ * n_g).vector();
|
||||
}
|
||||
|
||||
Rot3 bRn() const {return bRn_; }
|
||||
Vector x_g() const { return x_g_; }
|
||||
Vector x_a() const { return x_a_; }
|
||||
|
||||
/**
|
||||
* Initialize the first Mechanization state
|
||||
* @param U a list of gyro measurement vectors
|
||||
* @param F a list of accelerometer measurement vectors
|
||||
*/
|
||||
static Mechanization_bRn2 initializeVector(const std::list<Vector>& U,
|
||||
const std::list<Vector>& F, const double g_e = 0);
|
||||
|
||||
static Mechanization_bRn2 initialize(const Matrix& U,
|
||||
const Matrix& F, const double g_e = 0);
|
||||
|
||||
/**
|
||||
* Correct AHRS full state given error state dx
|
||||
* @param obj The current state
|
||||
* @param dx The error used to correct and return a new state
|
||||
*/
|
||||
Mechanization_bRn2 correct(const Vector& dx) const;
|
||||
|
||||
/**
|
||||
* Integrate to get new state
|
||||
* @param obj The current state
|
||||
* @param u gyro measurement to integrate
|
||||
* @param dt time elapsed since previous state in seconds
|
||||
*/
|
||||
Mechanization_bRn2 integrate(const Vector& u, const double dt) const;
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const {
|
||||
bRn_.print(s + ".R");
|
||||
|
||||
gtsam::print(x_g_, s + ".x_g");
|
||||
gtsam::print(x_a_, s + ".x_a");
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,147 @@
|
|||
(* Content-type: application/vnd.wolfram.mathematica *)
|
||||
|
||||
(*** Wolfram Notebook File ***)
|
||||
(* http://www.wolfram.com/nb *)
|
||||
|
||||
(* CreatedBy='Mathematica 8.0' *)
|
||||
|
||||
(*CacheID: 234*)
|
||||
(* Internal cache information:
|
||||
NotebookFileLineBreakTest
|
||||
NotebookFileLineBreakTest
|
||||
NotebookDataPosition[ 157, 7]
|
||||
NotebookDataLength[ 3876, 137]
|
||||
NotebookOptionsPosition[ 3514, 119]
|
||||
NotebookOutlinePosition[ 3867, 135]
|
||||
CellTagsIndexPosition[ 3824, 132]
|
||||
WindowFrame->Normal*)
|
||||
|
||||
(* Beginning of Notebook Content *)
|
||||
Notebook[{
|
||||
Cell[BoxData[{
|
||||
RowBox[{
|
||||
RowBox[{"roll", "=",
|
||||
RowBox[{"ArcTan", "[",
|
||||
RowBox[{"g3", ",", "g2"}], "]"}]}], ";"}], "\n",
|
||||
RowBox[{
|
||||
RowBox[{"pitch", "=",
|
||||
RowBox[{"ArcTan", "[",
|
||||
RowBox[{
|
||||
RowBox[{"-", "g1"}], ",",
|
||||
SqrtBox[
|
||||
RowBox[{
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}]]}], "]"}]}], ";"}], "\n"}], "Input",
|
||||
CellChangeTimes->{{3.535838264320565*^9, 3.535838380686434*^9}, {
|
||||
3.53583852660116*^9, 3.5358385374458313`*^9}, {3.5358391717151127`*^9,
|
||||
3.535839173489215*^9}}],
|
||||
|
||||
Cell[CellGroupData[{
|
||||
|
||||
Cell[BoxData[
|
||||
RowBox[{
|
||||
RowBox[{"(", "\[NoBreak]", GridBox[{
|
||||
{
|
||||
RowBox[{"D", "[",
|
||||
RowBox[{"roll", ",", "g1"}], "]"}],
|
||||
RowBox[{"D", "[",
|
||||
RowBox[{"roll", ",", "g2"}], "]"}],
|
||||
RowBox[{"D", "[",
|
||||
RowBox[{"roll", ",", "g3"}], "]"}]},
|
||||
{
|
||||
RowBox[{"D", "[",
|
||||
RowBox[{"pitch", ",", "g1"}], "]"}],
|
||||
RowBox[{"D", "[",
|
||||
RowBox[{"pitch", ",", "g2"}], "]"}],
|
||||
RowBox[{"D", "[",
|
||||
RowBox[{"pitch", ",", "g3"}], "]"}]},
|
||||
{"0", "0", "0"}
|
||||
}], "\[NoBreak]", ")"}], "//", "FullSimplify"}]], "Input",
|
||||
CellChangeTimes->{{3.535838384786139*^9, 3.535838492987266*^9}, {
|
||||
3.535838580947555*^9, 3.535838595178028*^9}}],
|
||||
|
||||
Cell[BoxData[
|
||||
RowBox[{"{",
|
||||
RowBox[{
|
||||
RowBox[{"{",
|
||||
RowBox[{"0", ",",
|
||||
FractionBox["g3",
|
||||
RowBox[{
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}]], ",",
|
||||
RowBox[{"-",
|
||||
FractionBox["g2",
|
||||
RowBox[{
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}]]}]}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{
|
||||
FractionBox[
|
||||
SqrtBox[
|
||||
RowBox[{
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}]],
|
||||
RowBox[{
|
||||
SuperscriptBox["g1", "2"], "+",
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}]], ",",
|
||||
RowBox[{"-",
|
||||
FractionBox[
|
||||
RowBox[{"g1", " ", "g2"}],
|
||||
RowBox[{
|
||||
SqrtBox[
|
||||
RowBox[{
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}]], " ",
|
||||
RowBox[{"(",
|
||||
RowBox[{
|
||||
SuperscriptBox["g1", "2"], "+",
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}], ")"}]}]]}], ",",
|
||||
RowBox[{"-",
|
||||
FractionBox[
|
||||
RowBox[{"g1", " ", "g3"}],
|
||||
RowBox[{
|
||||
SqrtBox[
|
||||
RowBox[{
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}]], " ",
|
||||
RowBox[{"(",
|
||||
RowBox[{
|
||||
SuperscriptBox["g1", "2"], "+",
|
||||
SuperscriptBox["g2", "2"], "+",
|
||||
SuperscriptBox["g3", "2"]}], ")"}]}]]}]}], "}"}], ",",
|
||||
RowBox[{"{",
|
||||
RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output",
|
||||
CellChangeTimes->{{3.5358383879873047`*^9, 3.535838493685678*^9},
|
||||
3.53583853974876*^9, {3.535838581771779*^9, 3.535838595765464*^9}}]
|
||||
}, Open ]]
|
||||
},
|
||||
WindowSize->{740, 752},
|
||||
WindowMargins->{{4, Automatic}, {Automatic, 4}},
|
||||
FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \
|
||||
2011)",
|
||||
StyleDefinitions->"Default.nb"
|
||||
]
|
||||
(* End of Notebook Content *)
|
||||
|
||||
(* Internal cache information *)
|
||||
(*CellTagsOutline
|
||||
CellTagsIndex->{}
|
||||
*)
|
||||
(*CellTagsIndex
|
||||
CellTagsIndex->{}
|
||||
*)
|
||||
(*NotebookFileOutline
|
||||
Notebook[{
|
||||
Cell[557, 20, 544, 16, 75, "Input"],
|
||||
Cell[CellGroupData[{
|
||||
Cell[1126, 40, 692, 20, 61, "Input"],
|
||||
Cell[1821, 62, 1677, 54, 109, "Output"]
|
||||
}, Open ]]
|
||||
}
|
||||
]
|
||||
*)
|
||||
|
||||
(* End of internal cache information *)
|
||||
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
/*
|
||||
* @file testAHRS.cpp
|
||||
* @brief Test AHRS
|
||||
* @author Frank Dellaert
|
||||
* @author Chris Beall
|
||||
*/
|
||||
|
||||
#include <list>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include "../AHRS.h"
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// stationary interval of gyro U and acc F
|
||||
Matrix stationaryU = trans(Matrix_(3,3,-0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003));
|
||||
Matrix stationaryF = trans(Matrix_(3,3,0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021));
|
||||
double g_e = 9.7963; // Atlanta
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (AHRS, cov) {
|
||||
|
||||
// samples stored by row
|
||||
Matrix A = Matrix_(4, 3,
|
||||
1.0, 2.0, 3.0,
|
||||
5.0, 7.0, 0.0,
|
||||
9.0, 4.0, 7.0,
|
||||
6.0, 3.0, 2.0);
|
||||
|
||||
Matrix actual = cov(trans(A));
|
||||
Matrix expected = Matrix_(3, 3,
|
||||
10.9167, 2.3333, 5.0000,
|
||||
2.3333, 4.6667, -2.6667,
|
||||
5.0000, -2.6667, 8.6667);
|
||||
|
||||
EXPECT(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (AHRS, covU) {
|
||||
|
||||
Matrix actual = cov(10000*stationaryU);
|
||||
Matrix expected = Matrix_(3, 3,
|
||||
33.3333333, -1.66666667, 53.3333333,
|
||||
-1.66666667, 0.333333333, -5.16666667,
|
||||
53.3333333, -5.16666667, 110.333333);
|
||||
|
||||
EXPECT(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (AHRS, covF) {
|
||||
|
||||
Matrix actual = cov(100*stationaryF);
|
||||
Matrix expected = Matrix_(3, 3,
|
||||
63.3808333, -0.432166667, -48.1706667,
|
||||
-0.432166667, 8.31053333, -16.6792667,
|
||||
-48.1706667, -16.6792667, 71.4297333);
|
||||
|
||||
EXPECT(assert_equal(expected, actual, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (AHRS, constructor) {
|
||||
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* TODO: currently fails because of problem with ill-conditioned system
|
||||
TEST (AHRS, init) {
|
||||
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
||||
std::pair<Mechanization_bRn2, KalmanFilter::State> result = ahrs.initialize(g_e);
|
||||
}
|
||||
*/
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue