apollo::perception::camera::KalmanFilterConstVelocity类 参考

#include <kalman_filter.h>

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

Public 成员函数
	KalmanFilterConstVelocity ()
void	Init (const Eigen::VectorXd &x)
void	Predict (float delta_t)
void	Correct (const Eigen::VectorXd &z)
Eigen::Vector4d	get_state () const
void	MagicPosition (const Eigen::VectorXd &pos)
void	MagicVelocity (const Eigen::VectorXd &vel)

Public 属性
Eigen::Matrix4d	variance_
Eigen::Matrix2d	measure_noise_
Eigen::Matrix4d	process_noise_
Eigen::Matrix< double, 4, 1 >	predict_state_
double	likelihood_ = 1.0

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

详细描述

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

构造及析构函数说明

KalmanFilterConstVelocity()

apollo::perception::camera::KalmanFilterConstVelocity::KalmanFilterConstVelocity

(

)

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

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

成员函数说明

Correct()

void apollo::perception::camera::KalmanFilterConstVelocity::Correct

(

const Eigen::VectorXd &

z

)

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

                                                              {
  if (inited_) {
    Eigen::Vector2d measure;
    measure << z[0], z[1];
    // measurement covariance: S = H*P*H^T + R
    Eigen::Matrix2d 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_);
 
    // compute likelihood
    auto residual = measure - predict_state_.head(2);
    likelihood_ =
        std::exp(-0.5 * residual.transpose() * cov.inverse() * residual) /
        std::sqrt(2 * M_PI * cov.determinant());
  } else {
    Init(z);
  }
}

get_state()

Eigen::Vector4d apollo::perception::camera::KalmanFilterConstVelocity::get_state

(

)

const

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

{ return state_; }

Init()

void apollo::perception::camera::KalmanFilterConstVelocity::Init

(

const Eigen::VectorXd &

x

)

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

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

MagicPosition()

void apollo::perception::camera::KalmanFilterConstVelocity::MagicPosition

(

const Eigen::VectorXd &

pos

)

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

                                                                      {
  state_(0) = pos(0);
  state_(1) = pos(1);
}

MagicVelocity()

void apollo::perception::camera::KalmanFilterConstVelocity::MagicVelocity

(

const Eigen::VectorXd &

vel

)

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

                                                                      {
  state_(2) = vel(0);
  state_(3) = vel(1);
}

Predict()

void apollo::perception::camera::KalmanFilterConstVelocity::Predict

(

float

delta_t

)

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

                                                     {
  if (inited_) {
    state_transition_matrix_(0, 2) = delta_t;
    state_transition_matrix_(1, 3) = delta_t;
    state_ = state_transition_matrix_ * state_;
    predict_state_ = state_;
    variance_ = state_transition_matrix_ * variance_ *
                    state_transition_matrix_.transpose() +
                process_noise_;
  }
}

类成员变量说明

inited_

bool apollo::perception::camera::KalmanFilterConstVelocity::inited_

protected

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

kalman_gain_

Eigen::Matrix<double, 4, 2> apollo::perception::camera::KalmanFilterConstVelocity::kalman_gain_

protected

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

likelihood_

double apollo::perception::camera::KalmanFilterConstVelocity::likelihood_ = 1.0

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

measure_matrix_

Eigen::Matrix<double, 2, 4> apollo::perception::camera::KalmanFilterConstVelocity::measure_matrix_

protected

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

measure_noise_

Eigen::Matrix2d apollo::perception::camera::KalmanFilterConstVelocity::measure_noise_

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

predict_state_

Eigen::Matrix<double, 4, 1> apollo::perception::camera::KalmanFilterConstVelocity::predict_state_

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

process_noise_

Eigen::Matrix4d apollo::perception::camera::KalmanFilterConstVelocity::process_noise_

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

state_

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

protected

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

state_transition_matrix_

Eigen::Matrix4d apollo::perception::camera::KalmanFilterConstVelocity::state_transition_matrix_

protected

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

variance_

Eigen::Matrix4d apollo::perception::camera::KalmanFilterConstVelocity::variance_

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

---

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

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