[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag/tag constantvelocity.h kalmanfilter.h
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] tag/tag constantvelocity.h kalmanfilter.h |
Date: |
Thu, 14 Dec 2006 14:00:46 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Gerhard Reitmayr <gerhard> 06/12/14 14:00:46
Modified files:
tag : constantvelocity.h kalmanfilter.h
Log message:
optimizations through returning references instead of temporary return
values and using reference variables in the filter
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/constantvelocity.h?cvsroot=toon&r1=1.2&r2=1.3
http://cvs.savannah.gnu.org/viewcvs/tag/tag/kalmanfilter.h?cvsroot=toon&r1=1.7&r2=1.8
Patches:
Index: constantvelocity.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/constantvelocity.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- constantvelocity.h 5 Jun 2006 15:19:30 -0000 1.2
+++ constantvelocity.h 14 Dec 2006 14:00:46 -0000 1.3
@@ -17,7 +17,7 @@
/// @ingroup constantvelocitygroup
class State {
public:
- State(void){
+ inline State(void){
reset();
}
@@ -54,23 +54,30 @@
class Model {
public:
TooN::Vector<State::STATE_DIMENSION> sigma;
+ TooN::Matrix<State::STATE_DIMENSION> jacobian;
+ TooN::Matrix<State::STATE_DIMENSION> noise;
// dampening of velocity
double damp;
Model(void){
TooN::Zero(sigma);
damp = 1;
+ TooN::Identity(jacobian);
+ TooN::Zero(noise);
}
// Jacobian has pos, rot, vel, angularVel in this order
- TooN::Matrix<State::STATE_DIMENSION> getJacobian(const State & state,
double dt){
- TooN::Matrix<State::STATE_DIMENSION> result;
- TooN::Identity(result);
- TooN::Identity(result.slice<0,6,6,6>(), dt);
- return result;
+ inline const TooN::Matrix<State::STATE_DIMENSION> & getJacobian(const
State & state, const double dt) {
+ jacobian(0,6) = dt;
+ jacobian(1,7) = dt;
+ jacobian(2,8) = dt;
+ jacobian(3,9) = dt;
+ jacobian(4,10) = dt;
+ jacobian(5,11) = dt;
+ return jacobian;
}
- void updateState( State & state, const double dt ){
+ inline void updateState( State & state, const double dt ){
// full velocity vector
TooN::Vector<6> vel;
vel.slice<0,3>() = state.velocity;
@@ -84,20 +91,18 @@
state.angularVelocity *= attenuation;
}
- TooN::Matrix<State::STATE_DIMENSION> getNoiseCovariance( double dt ){
- TooN::Matrix<State::STATE_DIMENSION> result;
- TooN::Zero(result);
- double dt2 = dt * dt * 0.5;
- double dt3 = dt * dt * dt * 0.3333333333333;
+ inline const TooN::Matrix<State::STATE_DIMENSION> & getNoiseCovariance(
const double dt ){
+ const double dt2 = dt * dt * 0.5;
+ const double dt3 = dt * dt * dt * 0.3333333333333;
for(unsigned int i = 0; i < 6; i++){
- result(i,i) = dt * sigma[i] + dt3 * sigma[i+6];
- result(i+6,i) = result(i,i+6) = dt2 * sigma[i+6];
- result(i+6, i+6) = dt * sigma[i+6];
+ noise(i,i) = dt * sigma[i] + dt3 * sigma[i+6];
+ noise(i+6,i) = noise(i,i+6) = dt2 * sigma[i+6];
+ noise(i+6, i+6) = dt * sigma[i+6];
}
- return result;
+ return noise;
}
- void updateFromMeasurement( State & state, const
TooN::Vector<State::STATE_DIMENSION> & innovation ){
+ inline void updateFromMeasurement( State & state, const
TooN::Vector<State::STATE_DIMENSION> & innovation ){
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.7
retrieving revision 1.8
diff -u -b -r1.7 -r1.8
--- kalmanfilter.h 5 Dec 2006 17:43:10 -0000 1.7
+++ kalmanfilter.h 14 Dec 2006 14:00:46 -0000 1.8
@@ -24,11 +24,11 @@
class Model {
public:
// return process model jacobian for a given state and time delta
(typically A)
- TooN::Matrix<State::STATE_DIMENSION> getJacobian(const State & state,
double dt);
+ const TooN::Matrix<State::STATE_DIMENSION> & getJacobian(const State &
state, double dt);
// update the state for a given time delta (not all states are actually x
= Ax, therefore this is a function)
void updateState( State & state, const double dt );
// return process noise matrix for given time delta (typically Q)
- TooN::Matrix<State::STATE_DIMENSION> getNoiseCovariance( double dt );
+ const TooN::Matrix<State::STATE_DIMENSION> & getNoiseCovariance( double dt
);
// update the state from an innovation. the innovation was computed by the
filter based on measurement etc.
void updateFromMeasurement( State & state, const
TooN::Vector<State::STATE_DIMENSION> & innovation );
};
@@ -41,11 +41,11 @@
public:
static const int M_DIMENSION = ??; // dimension of measurement
// return measurement jacobian, from state -> measurement
- Matrix<M_DIMENSION,State::STATE_DIMENSION> getMeasurementJacobian( const
State & state );
+ const Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian(
const State & state );
// return measurement noise covariance
- Matrix<M_DIMENSION> getMeasurementCovariance( const State & state );
+ const Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state
);
// return the innovation, the difference between actual measurement and
the measurement prediction based on the state
- Vector<M_DIMENSION> getInnovation( const State & state );
+ const Vector<M_DIMENSION> & getInnovation( const State & state );
};
@endcode
All of the member functions take the state as parameters, because the returned
values are typically
@@ -71,6 +71,9 @@
filter.filter(m);
}
@endcode
+
+Note, that all the return values from the various classes are const
references. This avoids any unnecessary copying of data.
+You can also return types that may be stored in const references, such as
non-const references and return values.
*/
/**
@@ -91,21 +94,20 @@
/// predicts the state by applying the process model over the time
interval dt
/// @param[in] dt time interval
void predict(double dt){
- TooN::Matrix<State::STATE_DIMENSION> A = model.getJacobian( state, dt
);
model.updateState( state, dt );
- state.covariance = TooN::transformCovariance(A, state.covariance) +
model.getNoiseCovariance( dt );
+ state.covariance = TooN::transformCovariance(model.getJacobian( state,
dt ), state.covariance) + model.getNoiseCovariance( dt );
TooN::Symmetrize(state.covariance);
}
/// incorporates a measurement
/// @param[in] m the measurement to add to the filter state
template<class Measurement> void filter(Measurement & m){
- TooN::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> H =
m.getMeasurementJacobian( state );
- TooN::Matrix<Measurement::M_DIMENSION> R = m.getMeasurementCovariance(
state );
+ 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();
- TooN::Vector<Measurement::M_DIMENSION> innovation = m.getInnovation(
state );
+ const TooN::Vector<Measurement::M_DIMENSION> & innovation =
m.getInnovation( state );
TooN::Vector<State::STATE_DIMENSION> stateInnovation = K * innovation;
model.updateFromMeasurement( state, stateInnovation );
state.covariance = (identity - K * H) * state.covariance;
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag/tag constantvelocity.h kalmanfilter.h,
Gerhard Reitmayr <=