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; } } }
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; }
// 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; };