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;
        }
    }
}
Exemple #2
0
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;
}
Exemple #3
0
  // 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;
  };