apollo::perception::camera::ExtendedKalmanFilter类 参考

#include <kalman_filter.h>

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

Public 成员函数
	ExtendedKalmanFilter ()
void	Init ()
void	Init (const Eigen::VectorXd &x)
void	Predict (float delta_t)
void	Correct (const Eigen::VectorXd &z)
Eigen::Vector4d	get_state () const

Public 属性
Eigen::Matrix4d	variance_
Eigen::Matrix3d	measure_noise_
Eigen::Matrix4d	process_noise_

Protected 属性
Eigen::Matrix4d	state_transition_matrix_
Eigen::Matrix< double, 4, 1 >	state_
Eigen::Matrix< double, 3, 4 >	measure_matrix_
Eigen::Matrix< double, 4, 3 >	kalman_gain_
bool	inited_

详细描述

在文件 kalman_filter.h 第 102 行定义.

构造及析构函数说明

ExtendedKalmanFilter()

apollo::perception::camera::ExtendedKalmanFilter::ExtendedKalmanFilter ( )

inline

在文件 kalman_filter.h 第 107 行定义.

: inited_(false) {}

成员函数说明

Correct()

void apollo::perception::camera::ExtendedKalmanFilter::Correct

(

const Eigen::VectorXd &

z

)

在文件 kalman_filter.cc 第 123 行定义.

                                                         {
  if (inited_) {
    Eigen::Vector3d measure;
    measure << z[0], z[1], z[2];
    Eigen::Matrix3d cov =
        measure_matrix_ * variance_ * measure_matrix_.transpose() +
        measure_noise_;
 
    kalman_gain_ = variance_ * measure_matrix_.transpose() * cov.inverse();
    variance_ = variance_ - kalman_gain_ * measure_matrix_ * variance_;
    state_ = state_ + kalman_gain_ * (measure - measure_matrix_ * state_);
  } else {
    inited_ = true;
    state_ << z[0], z[1], 0, z[2];
  }
}

get_state()

Eigen::Vector4d apollo::perception::camera::ExtendedKalmanFilter::get_state

(

)

const

在文件 kalman_filter.cc 第 140 行定义.

{ return state_; }

Init() [1/2]

void apollo::perception::camera::ExtendedKalmanFilter::Init

(

)

在文件 kalman_filter.cc 第 88 行定义.

                                {
  // other value should be changed in predict
  state_transition_matrix_.setIdentity();
  measure_matrix_ << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
  variance_.setIdentity();
  measure_noise_.setIdentity();
  process_noise_.setIdentity();
  inited_ = false;
}

Init() [2/2]

void apollo::perception::camera::ExtendedKalmanFilter::Init

(

const Eigen::VectorXd &

x

)

在文件 kalman_filter.cc 第 98 行定义.

                                                      {
  Init();
  state_ << x(0), x(1), 0, x(2);
  inited_ = true;
}

Predict()

void apollo::perception::camera::ExtendedKalmanFilter::Predict

(

float

delta_t

)

在文件 kalman_filter.cc 第 104 行定义.

                                                {
  if (inited_) {
    float sin_theta = static_cast<float>(std::sin(state_(3)));
    float cos_theta = static_cast<float>(std::cos(state_(3)));
    state_transition_matrix_(0, 2) = delta_t * cos_theta;
    state_transition_matrix_(0, 3) = -delta_t * state_(2) * sin_theta;
    state_transition_matrix_(1, 2) = delta_t * sin_theta;
    state_transition_matrix_(1, 3) = delta_t * state_(2) * cos_theta;
    state_(0) += state_(2) * cos_theta * delta_t;
    state_(1) += state_(2) * sin_theta * delta_t;
    //    No change
    //    state_(2) = state_(2);
    //    state_(3) = state_(3);
    variance_ = state_transition_matrix_ * variance_ *
                    state_transition_matrix_.transpose() +
                process_noise_;
  }
}

类成员变量说明

inited_

bool apollo::perception::camera::ExtendedKalmanFilter::inited_

protected

在文件 kalman_filter.h 第 134 行定义.

kalman_gain_

Eigen::Matrix<double, 4, 3> apollo::perception::camera::ExtendedKalmanFilter::kalman_gain_

protected

在文件 kalman_filter.h 第 133 行定义.

measure_matrix_

Eigen::Matrix<double, 3, 4> apollo::perception::camera::ExtendedKalmanFilter::measure_matrix_

protected

在文件 kalman_filter.h 第 132 行定义.

measure_noise_

Eigen::Matrix3d apollo::perception::camera::ExtendedKalmanFilter::measure_noise_

在文件 kalman_filter.h 第 119 行定义.

process_noise_

Eigen::Matrix4d apollo::perception::camera::ExtendedKalmanFilter::process_noise_

在文件 kalman_filter.h 第 120 行定义.

state_

Eigen::Matrix<double, 4, 1> apollo::perception::camera::ExtendedKalmanFilter::state_

protected

在文件 kalman_filter.h 第 131 行定义.

state_transition_matrix_

Eigen::Matrix4d apollo::perception::camera::ExtendedKalmanFilter::state_transition_matrix_

protected

在文件 kalman_filter.h 第 129 行定义.

variance_

Eigen::Matrix4d apollo::perception::camera::ExtendedKalmanFilter::variance_

在文件 kalman_filter.h 第 118 行定义.

---

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

• modules/perception/camera_tracking/common/kalman_filter.h
• modules/perception/camera_tracking/common/kalman_filter.cc