apollo::perception::camera::AdaptiveKalmanFilter类 参考

#include <adaptive_kalman_filter.h>

类 apollo::perception::camera::AdaptiveKalmanFilter 继承关系图:

apollo::perception::camera::AdaptiveKalmanFilter 的协作图:

Public 成员函数
	AdaptiveKalmanFilter ()
	~AdaptiveKalmanFilter ()
void	Init (const base::Object &object) override
	Init base filter config
Eigen::VectorXd	Predict (const double time_diff) override
	Predict the state of the object after time_diff
Eigen::VectorXd	UpdateWithObject (const base::Object &new_object, double time_diff) override
	Update the state of the target based on the observations
void	GetState (Eigen::Vector3d *anchor_point, Eigen::Vector3d *velocity)
	Get the State object
Eigen::Matrix4d	GetCovarianceMatrix () override
	Get the Covariance Matrix object
Public 成员函数 继承自 apollo::perception::camera::BaseFilter
	BaseFilter ()
	Construct a new Base Filter object
virtual	~BaseFilter ()
std::string	Name ()
	The name of the camera base Filter

静态 Public 成员函数
static void	SetQMatrixRatio (double q_matrix_ratio)
	Set the QMatrix Ratio

额外继承的成员函数
Protected 属性 继承自 apollo::perception::camera::BaseFilter
std::string	name_

详细描述

在文件 adaptive_kalman_filter.h 第 26 行定义.

构造及析构函数说明

AdaptiveKalmanFilter()

apollo::perception::camera::AdaptiveKalmanFilter::AdaptiveKalmanFilter

(

)

在文件 adaptive_kalman_filter.cc 第 26 行定义.

{ name_ = "AdaptiveKalmanFilter"; }

~AdaptiveKalmanFilter()

apollo::perception::camera::AdaptiveKalmanFilter::~AdaptiveKalmanFilter

(

)

在文件 adaptive_kalman_filter.cc 第 28 行定义.

{}

成员函数说明

GetCovarianceMatrix()

Eigen::Matrix4d apollo::perception::camera::AdaptiveKalmanFilter::GetCovarianceMatrix ( )

inlineoverridevirtual

Get the Covariance Matrix object

返回

Eigen::Matrix4d

实现了 apollo::perception::camera::BaseFilter.

在文件 adaptive_kalman_filter.h 第 72 行定义.

{ return p_matrix_; }

GetState()

void apollo::perception::camera::AdaptiveKalmanFilter::GetState ( Eigen::Vector3d *  anchor_point, Eigen::Vector3d *  velocity  )

virtual

Get the State object

参数

anchor_point
velocity

实现了 apollo::perception::camera::BaseFilter.

在文件 adaptive_kalman_filter.cc 第 109 行定义.

                                                             {
  if (anchor_point == nullptr) {
    AERROR << "anchor_point is not available";
    return;
  }
  if (velocity == nullptr) {
    AERROR << "velocity is not available";
    return;
  }
  *anchor_point = belief_anchor_point_;
  *velocity = belief_velocity_;
}

Init()

void apollo::perception::camera::AdaptiveKalmanFilter::Init ( const base::Object &  object )

overridevirtual

Init base filter config

参数

object

实现了 apollo::perception::camera::BaseFilter.

在文件 adaptive_kalman_filter.cc 第 30 行定义.

                                                        {
  belief_anchor_point_ = object.center;
  belief_velocity_ = object.velocity.cast<double>();
  a_matrix_.setIdentity();
  // initialize states to the states of the detected obstacle
  posteriori_state_(0) = belief_anchor_point_(0);
  posteriori_state_(1) = belief_anchor_point_(1);
  posteriori_state_(2) = belief_velocity_(0);
  posteriori_state_(3) = belief_velocity_(1);
  priori_state_ = posteriori_state_;
  q_matrix_.setIdentity();
  q_matrix_ *= s_q_matrix_ratio_;
  r_matrix_.setIdentity();
  r_matrix_.topLeftCorner(2, 2) =
      object.center_uncertainty.topLeftCorner(2, 2).cast<double>();
  r_matrix_.block<2, 2>(2, 2) =
      object.velocity_uncertainty.topLeftCorner(2, 2).cast<double>();
  p_matrix_.setIdentity();
  p_matrix_.topLeftCorner(2, 2) =
      object.center_uncertainty.topLeftCorner(2, 2).cast<double>();
  p_matrix_.block<2, 2>(2, 2) =
      object.velocity_uncertainty.topLeftCorner(2, 2).cast<double>();
  c_matrix_.setIdentity();
}

Predict()

Eigen::VectorXd apollo::perception::camera::AdaptiveKalmanFilter::Predict ( const double  time_diff )

overridevirtual

Predict the state of the object after time_diff

参数

time_diff

返回

Eigen::VectorXd

实现了 apollo::perception::camera::BaseFilter.

在文件 adaptive_kalman_filter.cc 第 55 行定义.

                                                                  {
  Eigen::VectorXd state;
  state.resize(4);
  state[0] = belief_anchor_point_[0] + belief_velocity_[0] * time_diff;
  state[1] = belief_anchor_point_[1] + belief_velocity_[1] * time_diff;
  state[2] = belief_velocity_[0];
  state[3] = belief_velocity_[1];
  return state;
}

SetQMatrixRatio()

static void apollo::perception::camera::AdaptiveKalmanFilter::SetQMatrixRatio ( double  q_matrix_ratio )

inlinestatic

Set the QMatrix Ratio

参数

q_matrix_ratio

在文件 adaptive_kalman_filter.h 第 79 行定义.

                                                     {
    s_q_matrix_ratio_ = q_matrix_ratio;
  }

UpdateWithObject()

Eigen::VectorXd apollo::perception::camera::AdaptiveKalmanFilter::UpdateWithObject ( const base::Object &  new_object, double  time_diff  )

overridevirtual

Update the state of the target based on the observations

参数

new_object
time_diff

返回

Eigen::VectorXd

实现了 apollo::perception::camera::BaseFilter.

在文件 adaptive_kalman_filter.cc 第 65 行定义.

                                                    {
  // predict and then correct
  a_matrix_.setIdentity();
  a_matrix_(0, 2) = time_diff;
  a_matrix_(1, 3) = time_diff;
  priori_state_ = a_matrix_ * posteriori_state_;
  p_matrix_ = ((a_matrix_ * p_matrix_) * a_matrix_.transpose()) + q_matrix_;
  belief_anchor_point_ = new_object.center;
  belief_velocity_ = new_object.velocity.cast<double>();
  Eigen::Vector4d measurement;
  measurement(0) = belief_anchor_point_(0);
  measurement(1) = belief_anchor_point_(1);
  measurement(2) = belief_velocity_(0);
  measurement(3) = belief_velocity_(1);
  r_matrix_.setIdentity();
  r_matrix_.topLeftCorner(2, 2) =
      new_object.center_uncertainty.topLeftCorner(2, 2).cast<double>();
  r_matrix_.block<2, 2>(2, 2) =
      new_object.velocity_uncertainty.topLeftCorner(2, 2).cast<double>();
  k_matrix_ =
      p_matrix_ * c_matrix_.transpose() *
      (c_matrix_ * p_matrix_ * c_matrix_.transpose() + r_matrix_).inverse();
  Eigen::Vector4d predict_measurement(priori_state_(0), priori_state_(1),
                                      priori_state_(2), priori_state_(3));
  posteriori_state_ =
      priori_state_ + k_matrix_ * (measurement - predict_measurement);
  p_matrix_ =
      (Eigen::Matrix4d::Identity() - k_matrix_ * c_matrix_) * p_matrix_ *
          (Eigen::Matrix4d::Identity() - k_matrix_ * c_matrix_).transpose() +
      k_matrix_ * r_matrix_ * k_matrix_.transpose();
  belief_anchor_point_(0) = posteriori_state_(0);
  belief_anchor_point_(1) = posteriori_state_(1);
  belief_velocity_(0) = posteriori_state_(2);
  belief_velocity_(1) = posteriori_state_(3);
  Eigen::VectorXd state;
  state.resize(4);
  state[0] = belief_anchor_point_[0];
  state[1] = belief_anchor_point_[1];
  state[2] = belief_velocity_[0];
  state[3] = belief_velocity_[1];
  return state;
}

---

该类的文档由以下文件生成:

• modules/perception/camera_detection_occupancy/tracker/filter/adaptive_kalman_filter.h
• modules/perception/camera_detection_occupancy/tracker/filter/adaptive_kalman_filter.cc