[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag/tag constantposition.h constantvelocity.h k...
|
From: |
Gerhard Reitmayr |
|
Subject: |
[Toon-members] tag/tag constantposition.h constantvelocity.h k... |
|
Date: |
Thu, 06 May 2010 12:47:00 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Gerhard Reitmayr <gerhard> 10/05/06 12:47:00
Modified files:
tag : constantposition.h constantvelocity.h
kalmanfilter.h measurements.h
Log message:
moved Kalman filter framework to the new age of TooN 2. Fixed all the
TooN 1.0isms, different update implementation using Cholesky for better
stability (keeps things symmetric) and no dependency on transformCovariance
anymore (a small performance penality in predict ...)
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/constantposition.h?cvsroot=toon&r1=1.1&r2=1.2
http://cvs.savannah.gnu.org/viewcvs/tag/tag/constantvelocity.h?cvsroot=toon&r1=1.5&r2=1.6
http://cvs.savannah.gnu.org/viewcvs/tag/tag/kalmanfilter.h?cvsroot=toon&r1=1.9&r2=1.10
http://cvs.savannah.gnu.org/viewcvs/tag/tag/measurements.h?cvsroot=toon&r1=1.4&r2=1.5
Patches:
Index: constantposition.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/constantposition.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- constantposition.h 31 May 2006 16:46:20 -0000 1.1
+++ constantposition.h 6 May 2010 12:47:00 -0000 1.2
@@ -22,12 +22,12 @@
}
void reset(void){
- position = TooN::SE3();
- TooN::Identity(covariance);
+ pose = TooN::SE3<>();
+ covariance = TooN::Identity;
}
static const int STATE_DIMENSION = 6;
- TooN::SE3 pose;
+ TooN::SE3<> pose;
TooN::Matrix<STATE_DIMENSION> covariance;
};
@@ -53,9 +53,9 @@
TooN::Matrix<State::STATE_DIMENSION> noise;
Model(void){
- TooN::Zero(sigma);
- TooN::Zero(noise);
- TooN::Identity(jacobian);
+ sigma = TooN::Zeros;
+ noise = TooN::Zeros;
+ jacobian = TooN::Identity;
}
/// Jacobian has pos, rot in this order
@@ -74,7 +74,7 @@
}
void updateFromMeasurement( State & state, const
TooN::Vector<State::STATE_DIMENSION> & innovation ){
- state.pose = TooN::SE3::exp(innovation) * state.pose;
+ state.pose = TooN::SE3<>::exp(innovation) * state.pose;
}
};
Index: constantvelocity.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/constantvelocity.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- constantvelocity.h 6 May 2010 10:44:07 -0000 1.5
+++ constantvelocity.h 6 May 2010 12:47:00 -0000 1.6
@@ -22,19 +22,19 @@
}
void reset(void){
- pose = TooN::SE3();
- TooN::Zero(angularVelocity);
- TooN::Zero(velocity);
- TooN::Identity(covariance);
+ pose = TooN::SE3<>();
+ angularVelocity = TooN::Zeros;
+ velocity = TooN::Zeros;
+ covariance = TooN::Identity;
}
void resetVelocity(void){
- TooN::Zero(angularVelocity);
- TooN::Zero(velocity);
+ angularVelocity = TooN::Zeros;
+ velocity = TooN::Zeros;
}
static const int STATE_DIMENSION = 12;
- TooN::SE3 pose;
+ TooN::SE3<> pose;
TooN::Vector<3> angularVelocity;
TooN::Vector<3> velocity;
TooN::Matrix<STATE_DIMENSION> covariance;
@@ -60,10 +60,10 @@
double damp;
Model(void){
- TooN::Zero(sigma);
+ sigma = TooN::Zeros;
damp = 1;
- TooN::Identity(jacobian);
- TooN::Zero(noise);
+ jacobian = TooN::Identity;
+ noise = TooN::Zeros;
}
// Jacobian has pos, rot, vel, angularVel in this order
@@ -84,7 +84,7 @@
vel.slice<3,3>() = state.angularVelocity;
// update translational components
- state.pose = TooN::SE3::exp(vel * dt) * state.pose;
+ state.pose = TooN::SE3<>::exp(vel * dt) * state.pose;
// dampen velocitys
double attenuation = pow(damp, dt);
state.velocity *= attenuation;
@@ -103,7 +103,7 @@
}
void updateFromMeasurement( State & state, const
TooN::Vector<State::STATE_DIMENSION> & innovation ){
- state.pose = TooN::SE3::exp(innovation.slice<0,6>()) * state.pose;
+ state.pose = TooN::SE3<>::exp(innovation.slice<0,6>()) * state.pose;
state.velocity = state.velocity + innovation.slice<6,3>();
state.angularVelocity = state.angularVelocity +
innovation.slice<9,3>();
}
Index: kalmanfilter.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/kalmanfilter.h,v
retrieving revision 1.9
retrieving revision 1.10
diff -u -b -r1.9 -r1.10
--- kalmanfilter.h 9 Dec 2008 20:33:42 -0000 1.9
+++ kalmanfilter.h 6 May 2010 12:47:00 -0000 1.10
@@ -3,7 +3,7 @@
#include <TooN/TooN.h>
#include <TooN/helpers.h>
-#include <TooN/LU.h>
+#include <TooN/Cholesky.h>
namespace tag {
@@ -88,13 +88,15 @@
typedef Model model_type;
KalmanFilter(){
- TooN::Identity(identity);
+ identity = TooN::Identity;
}
/// predicts the state by applying the process model over the time
interval dt
/// @param[in] dt time interval
void predict(double dt){
- state.covariance = TooN::transformCovariance(model.getJacobian( state,
dt ), state.covariance) + model.getNoiseCovariance( dt );
+ //state.covariance = TooN::transformCovariance(model.getJacobian(
state, dt ), state.covariance) + model.getNoiseCovariance( dt );
+ const TooN::Matrix<State::STATE_DIMENSION> & A = model.getJacobian(
state, dt );
+ state.covariance = A * state.covariance * A.T() +
model.getNoiseCovariance( dt );
TooN::Symmetrize(state.covariance);
model.updateState( state, dt );
}
@@ -104,14 +106,13 @@
template<class Measurement> void filter(Measurement & m){
const TooN::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> &
H = m.getMeasurementJacobian( state );
const TooN::Matrix<Measurement::M_DIMENSION> & R =
m.getMeasurementCovariance( state );
- TooN::Matrix<Measurement::M_DIMENSION> I =
TooN::transformCovariance(H, state.covariance) + R;
- TooN::LU<Measurement::M_DIMENSION> lu(I);
- TooN::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION> K =
state.covariance * H.T() * lu.get_inverse();
const TooN::Vector<Measurement::M_DIMENSION> & innovation =
m.getInnovation( state );
- TooN::Vector<State::STATE_DIMENSION> stateInnovation = K * innovation;
+ const TooN::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION>
P12 = state.covariance * H.T();
+ TooN::Cholesky<Measurement::M_DIMENSION> denom(H * P12 + R);
+ state.covariance = state.covariance - P12 * denom.backsub(P12.T());
+ // TooN::Symmetrize(state.covariance); // not necessary, above seems
to be good enough
+ const TooN::Vector<State::STATE_DIMENSION> stateInnovation = P12 *
denom.backsub(innovation);
model.updateFromMeasurement( state, stateInnovation );
- state.covariance = (identity - K * H) * state.covariance;
- TooN::Symmetrize( state.covariance );
}
/// identity matrix of the right size, used in the measurement equations
Index: measurements.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/measurements.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- measurements.h 14 Feb 2007 15:31:28 -0000 1.4
+++ measurements.h 6 May 2010 12:47:00 -0000 1.5
@@ -24,25 +24,25 @@
TooN::Vector<M_DIMENSION> measurement;
IncrementalPose(void){
- TooN::Identity(covariance);
- TooN::Zero(jacobian);
- TooN::Identity(jacobian.template slice<0,0,6,6>());
- TooN::Zero(measurement);
+ covariance = TooN::Identity;
+ jacobian = TooN::Zeros;
+ jacobian.template slice<0,0,6,6>() = TooN::Identity;
+ measurement = TooN::Zeros;
}
- inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ){
+ TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian(
const State & state ){
return jacobian;
}
- inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State &
state ){
+ TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state
){
return covariance;
}
- inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
+ TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
return measurement;
}
- inline void setMeasurement( const TooN::SE3 & increment ){
+ void setMeasurement( const TooN::SE3<> & increment ){
measurement = increment.ln();
}
};
@@ -57,25 +57,25 @@
TooN::Matrix<M_DIMENSION> covariance;
TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;
- TooN::SE3 measurement;
+ TooN::SE3<> measurement;
WorldPose(void){
- TooN::Identity(covariance);
- TooN::Zero(jacobian);
- TooN::Identity(jacobian.template slice<0,0,6,6>());
+ covariance = TooN::Identity;
+ jacobian = TooN::Zeros;
+ jacobian.template slice<0,0,6,6>() = TooN::Identity;
}
- inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ){
+ TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian(
const State & state ){
return jacobian;
}
- inline const TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const
State & state ) const {
+ const TooN::Matrix<M_DIMENSION> getMeasurementCovariance( const State &
state ) const {
TooN::Matrix<M_DIMENSION> localCovariance = covariance;
state.pose.adjoint(localCovariance);
return localCovariance;
}
- inline const TooN::Vector<M_DIMENSION> & getInnovation( const State &
state ) const {
+ const TooN::Vector<M_DIMENSION> getInnovation( const State & state ) const
{
return (measurement * state.pose.inverse()).ln();
}
};
@@ -93,36 +93,35 @@
TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;
WorldPosition(void){
- TooN::Identity(covariance);
- TooN::Zero(position);
- TooN::Zero(jacobian);
- TooN::Identity(jacobian.template slice<0,0,3,3>());
+ covariance = TooN::Identity;
+ position = TooN::Zeros;
+ jacobian = TooN::Zeros;
+ jacobian.template slice<0,0,3,3>() = TooN::Identity;
}
- inline const TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ) const {
+ const TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ) const {
return jacobian;
}
- inline const TooN::Matrix<M_DIMENSION> getMeasurementCovariance( const
State & state ) const {
- TooN::Matrix<2 * M_DIMENSION> localCovariance;
- TooN::Zero(localCovariance);
+ const TooN::Matrix<M_DIMENSION> getMeasurementCovariance( const State &
state ) const {
+ TooN::Matrix<2 * M_DIMENSION> localCovariance = TooN::Zeros;
localCovariance.template slice<0,0,3,3>() = covariance;
state.pose.adjoint(localCovariance);
return localCovariance.template slice<0,0,3,3>();
}
- inline const TooN::Vector<M_DIMENSION> getInnovation( const State & state
) const {
+ const TooN::Vector<M_DIMENSION> getInnovation( const State & state ) const
{
/// the negative vector corresponds to the left transformation to get
from the current reference
/// frame to the new reference frame where position is the origin.
return -(state.pose * position);
}
- inline void setCovariance( double sigma ){
- TooN::Identity(covariance, sigma);
+ void setCovariance( double sigma ){
+ covariance = TooN::Identity * sigma;
}
- inline void setCovariance( const TooN::Vector<M_DIMENSION> & sigma ){
- TooN::Zero(covariance);
+ void setCovariance( const TooN::Vector<M_DIMENSION> & sigma ){
+ covariance = TooN::Zeros;
for(int i = 0; i < M_DIMENSION; ++i){
covariance(i,i) = sigma[i];
}
@@ -142,29 +141,29 @@
TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;
AngularVelocity(void){
- TooN::Identity(covariance);
- TooN::Zero(gyro);
+ covariance = TooN::Identity;
+ gyro = TooN::Zeros;
// angularVelocity jacobian
- TooN::Zero(jacobian);
- TooN::Identity(jacobian.template slice<0,9,3,3>());
+ jacobian = TooN::Zeros;
+ jacobian.template slice<0,9,3,3>() = TooN::Identity;
}
- inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ){
+ TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian(
const State & state ){
return jacobian;
}
- inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State &
state ){
+ TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state
){
return covariance;
}
- inline TooN::Vector<M_DIMENSION> getInnovation( const State & state ){
+ TooN::Vector<M_DIMENSION> getInnovation( const State & state ){
// angular velocity
/// @todo think about the -gyro and either document or remove
return (-gyro - state.angularVelocity);
}
- inline void setCovariance( double sigma ){
- TooN::Identity(covariance, sigma);
+ void setCovariance( double sigma ){
+ covariance = TooN::Identity * sigma;
}
};
@@ -183,32 +182,31 @@
TooN::Matrix<M_DIMENSION> covariance;
WorldDirection(void){
- TooN::Identity(covariance);
- TooN::Zero(measurement);
- TooN::Zero(reference);
+ covariance = TooN::Identity;
+ measurement = TooN::Zeros;
+ reference = TooN::Zeros;
}
- inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ){
- TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> result;
- TooN::Zero(result);
+ TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> getMeasurementJacobian(
const State & state ){
+ TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> result = TooN::Zeros;
// direction jacobian
TooN::Vector<M_DIMENSION> local = state.pose.get_rotation() *
reference;
- result.template slice<0,3,3,1>() = TooN::SO3::generator_field(0, local
).as_col();
- result.template slice<0,4,3,1>() = TooN::SO3::generator_field(1, local
).as_col();
- result.template slice<0,5,3,1>() = TooN::SO3::generator_field(2, local
).as_col();
+ result.template slice<0,3,3,1>() = TooN::SO3<>::generator_field(0,
local ).as_col();
+ result.template slice<0,4,3,1>() = TooN::SO3<>::generator_field(1,
local ).as_col();
+ result.template slice<0,5,3,1>() = TooN::SO3<>::generator_field(2,
local ).as_col();
return result;
}
- inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State &
state ){
+ TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state
){
return covariance;
}
- inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
+ TooN::Vector<M_DIMENSION> getInnovation( const State & state ){
return (measurement - (state.pose.get_rotation() * reference));
}
- inline void setCovariance( double sigma ){
- TooN::Identity(covariance, sigma);
+ void setCovariance( double sigma ){
+ covariance = TooN::Identity * sigma;
}
};
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag/tag constantposition.h constantvelocity.h k...,
Gerhard Reitmayr <=