bool approxEqual(const MatrixWrapper::SymmetricMatrix& a, const MatrixWrapper::SymmetricMatrix& b, double epsilon)
{
if (a.rows() != b.rows()) return false;
if (a.columns() != b.columns()) return false;
for (unsigned int r=0; r<a.rows(); r++)
for (unsigned int c=0; c<a.columns(); c++)
if (!approxEqual(a(r+1, c+1), b(r+1,c+1),epsilon)) return false;
return true;
}
void BeaconKFNode::getCovarianceMatrix(std::string param_name, MatrixWrapper::SymmetricMatrix& m)
{
ros::NodeHandle private_nh("~");
XmlRpc::XmlRpcValue values;
private_nh.getParam(param_name, values);
if( values.getType() != XmlRpc::XmlRpcValue::TypeArray )
{
ROS_ERROR("BEACON LOCALIZER Unable to read covariance %s, not an array", param_name.c_str());
return;
}
if( values.size() < 6 )
{
ROS_ERROR("BEACON LOCALIZER Unable to read covariance %s, array too short: %d", param_name.c_str(), values.size());
return;
}
int i=0;
for (uint32_t row = 1; row <= m.rows(); ++row)
{
for (uint32_t column = row; column <= m.columns(); ++column)
{
double x;
if(i>=values.size())
{
ROS_ERROR("BEACON LOCALIZER Need at least 6 values, have %d", i);
return;
}
XmlRpc::XmlRpcValue value=values[i++];
if( value.getType() == XmlRpc::XmlRpcValue::TypeInt )
{
x=int(value);
}
else if(value.getType() == XmlRpc::XmlRpcValue::TypeDouble )
{
x=double(value);
}
else
{
std::string vstr = value;
ROS_ERROR("BEACON LOCALIZER Unable to read covariance matrix %s, value at %d is not a number: %s",
param_name.c_str(), i, vstr.c_str());
return;
}
m(row, column) = x;
}
}
}
// update filter correction
bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov)
{
assert(cov.columns() == 3);
// copy measurement
ColumnVector meas_vec(3);
for (unsigned int i=0; i<3; i++)
meas_vec(i+1) = meas[i];
// set covariance
((LinearAnalyticConditionalGaussian*)(meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov);
// update filter
bool res = filter_->Update(meas_model_, meas_vec);
if (!res) quality_ = 0;
else quality_ = calculateQuality();
return res;
};