FilterBase::FilterBase() :
state_(STATE_SIZE),
transferFunction_(STATE_SIZE, STATE_SIZE),
transferFunctionJacobian_(STATE_SIZE, STATE_SIZE),
estimateErrorCovariance_(STATE_SIZE, STATE_SIZE),
covarianceEpsilon_(STATE_SIZE, STATE_SIZE),
processNoiseCovariance_(STATE_SIZE, STATE_SIZE),
identity_(STATE_SIZE, STATE_SIZE),
pi_(3.141592653589793),
tau_(6.283185307179586),
debug_(false),
debugStream_(NULL)
{
initialized_ = false;
// Clear the state
state_.setZero();
// Prepare the invariant parts of the transfer
// function
transferFunction_.setIdentity();
// Clear the Jacobian
transferFunctionJacobian_.setZero();
// Prepare the invariant parts of the transfer
// function
estimateErrorCovariance_.setIdentity();
// We need the identity for the update equations
identity_.setIdentity();
// Set the epsilon matrix to be a matrix with small values on the diagonal
// It is used to maintain the positive-definite property of the covariance
covarianceEpsilon_.setIdentity();
covarianceEpsilon_ *= 0.001;
// Assume 30Hz from sensor data (configurable)
sensorTimeout_ = 0.033333333;
// Initialize our last update and measurement times
lastUpdateTime_ = 0;
lastMeasurementTime_ = 0;
// These can be overridden via the launch parameters,
// but we need default values.
processNoiseCovariance_.setZero();
processNoiseCovariance_(StateMemberX, StateMemberX) = 0.03;
processNoiseCovariance_(StateMemberY, StateMemberY) = 0.03;
processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.4;
processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.05;
processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.002;
processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.002;
processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.004;
}
FilterBase::FilterBase() :
state_(STATE_SIZE),
predictedState_(STATE_SIZE),
transferFunction_(STATE_SIZE, STATE_SIZE),
transferFunctionJacobian_(STATE_SIZE, STATE_SIZE),
estimateErrorCovariance_(STATE_SIZE, STATE_SIZE),
covarianceEpsilon_(STATE_SIZE, STATE_SIZE),
processNoiseCovariance_(STATE_SIZE, STATE_SIZE),
identity_(STATE_SIZE, STATE_SIZE),
debug_(false),
debugStream_(NULL)
{
initialized_ = false;
// Clear the state and predicted state
state_.setZero();
predictedState_.setZero();
// Prepare the invariant parts of the transfer
// function
transferFunction_.setIdentity();
// Clear the Jacobian
transferFunctionJacobian_.setZero();
// Set the estimate error covariance. We want our measurements
// to be accepted rapidly when the filter starts, so we should
// initialize the state's covariance with large values.
estimateErrorCovariance_.setIdentity();
estimateErrorCovariance_ *= 1e-9;
// We need the identity for the update equations
identity_.setIdentity();
// Set the epsilon matrix to be a matrix with small values on the diagonal
// It is used to maintain the positive-definite property of the covariance
covarianceEpsilon_.setIdentity();
covarianceEpsilon_ *= 0.001;
// Assume 30Hz from sensor data (configurable)
sensorTimeout_ = 0.033333333;
// Initialize our last update and measurement times
lastUpdateTime_ = 0;
lastMeasurementTime_ = 0;
// These can be overridden via the launch parameters,
// but we need default values.
processNoiseCovariance_.setZero();
processNoiseCovariance_(StateMemberX, StateMemberX) = 0.05;
processNoiseCovariance_(StateMemberY, StateMemberY) = 0.05;
processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.06;
processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.04;
processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.01;
processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.01;
processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.02;
processNoiseCovariance_(StateMemberAx, StateMemberAx) = 0.01;
processNoiseCovariance_(StateMemberAy, StateMemberAy) = 0.01;
processNoiseCovariance_(StateMemberAz, StateMemberAz) = 0.015;
}