激光雷达测量模型
参数定义
测量向量z
z是测量矢量。对于激光雷达传感器,矢量z是包含位置x和位置y测量。
测量矩阵H
H是将你对物体当前状态的信息投射到传感器测量空间的矩阵。对于激光雷达来说,这是一种奇特的说法,即我们丢弃状态变量中的速度信息,因为激光雷达传感器只测量位置:状态向量x是一个四维向量, 包含关于位置信息
矢量z将只包含[px,py],乘以hx,我们可以比较x,我们的信仰,和z,传感器测量值。
x向量中的素数符号代表什么意思呢?
Px’意味着您已经完成了预测步骤,但尚未完成测量步骤。换言之,这个物体是在位置Px, 经过一段时间 t_t 之后 。你计算出你认为物体将基于运动模型的位置,得到Px’
举个例子,如何找到正确的H矩阵,从4d状态投影到2d观察空间,如下所示:
为了获得我们的测量值(x和y),我们需要确定H矩阵。(x和y)是用H乘以状态向量得到的。
我们先来找出H矩阵的维数。这里我们有一个2乘1的矩阵,它来自m乘n乘h的矩阵乘以四行一列的矩阵。现在,从矩阵乘法中我们知道,第一个矩阵的列数应该等于第二个矩阵的行数,即4。并且第一个矩阵的行数与结果矩阵的行数相同,即2。所以我们的h是一个2行4列的矩阵。最后,我们把1和0放进去,使px和py坐标传播到结果z。
协方差矩阵R
测量噪声协方差矩阵R
现在,让我们来看一下协方差矩阵,R,它表示传感器测量中的不确定性。R矩阵的尺寸为正方形,其每边的长度与测量参数的数量相同。
对于激光传感器,我们有一个二维测量矢量。每个位置分量px,py都受到随机噪声的影响。所以我们的噪声向量ω与z的维数相同。它是一个平均值为零的分布,协方差矩阵是2×2,来自于垂直向量ω和它的转置的乘积。
其中R是测量噪声协方差矩阵;换句话说,矩阵R代表我们从激光传感器接收到的位置测量的不确定性。
通常,随机噪声测量矩阵的参数将由传感器制造商提供。对于扩展卡尔曼滤波项目,我们为雷达传感器和激光雷达传感器提供了R矩阵值。
记住,R中的非对角0表示噪声过程不相关。
编程实现激光跟踪
下面我们就来实现一个在二维坐标系中,利用Kalman滤波器进行追踪行人。
激光雷达测量能预测到行人的位置x和y,但是没有速度信息。
其中F矩阵和Q矩阵如下:
tracking.cpp
#include "tracking.h"
#include <iostream>
#include "Dense"
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::vector;
Tracking::Tracking() {
is_initialized_ = false;
previous_timestamp_ = 0;
// create a 4D state vector, we don't know yet the values of the x state
kf_.x_ = VectorXd(4);
// state covariance matrix P
kf_.P_ = MatrixXd(4, 4);
kf_.P_ << 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1000, 0,
0, 0, 0, 1000;
// measurement covariance
kf_.R_ = MatrixXd(2, 2);
kf_.R_ << 0.0225, 0,
0, 0.0225;
// measurement matrix
kf_.H_ = MatrixXd(2, 4);
kf_.H_ << 1, 0, 0, 0,
0, 1, 0, 0;
// the initial transition matrix F_
kf_.F_ = MatrixXd(4, 4);
kf_.F_ << 1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1;
// set the acceleration noise components
noise_ax = 5;
noise_ay = 5;
}
Tracking::~Tracking() {
}
// Process a single measurement
void Tracking::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
if (!is_initialized_) {
//cout << "Kalman Filter Initialization " << endl;
// set the state with the initial location and zero velocity
kf_.x_ << measurement_pack.raw_measurements_[0],
measurement_pack.raw_measurements_[1],
0,
0;
previous_timestamp_ = measurement_pack.timestamp_;
is_initialized_ = true;
return;
}
// compute the time elapsed between the current and previous measurements
// dt - expressed in seconds
float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
previous_timestamp_ = measurement_pack.timestamp_;
// TODO: YOUR CODE HERE
float dt_2 = dt * dt;
float dt_3 = dt_2 * dt;
float dt_4 = dt_3 * dt;
// 1. Modify the F matrix so that the time is integrated
kf_.F_(0,2) = dt;
kf_.F_(1,3) = dt;
// 2. Set the process covariance matrix Q
kf_.Q_ = MatrixXd(4, 4);
kf_.Q_ << dt_4 *noise_ax/4 , 0, dt_3 *noise_ax/2, 0,
0, dt_4 *noise_ay/4, 0, dt_3 *noise_ay/2,
dt_3 *noise_ax/2, 0, dt_2*noise_ax, 0,
0, dt_3 *noise_ay/2, 0, dt_2*noise_ay;
// 3. Call the Kalman Filter predict() function
kf_.Predict();
// 4. Call the Kalman Filter update() function
// with the most recent raw measurements_
kf_.Update(measurement_pack.raw_measurements_);
cout << "x_= " << kf_.x_ << endl;
cout << "P_= " << kf_.P_ << endl;
}
tracking.h
#ifndef TRACKING_H_
#define TRACKING_H_
#include <vector>
#include <string>
#include <fstream>
#include "kalman_filter.h"
#include "measurement_package.h"
class Tracking {
public:
Tracking();
virtual ~Tracking();
void ProcessMeasurement(const MeasurementPackage &measurement_pack);
KalmanFilter kf_;
private:
bool is_initialized_;
int64_t previous_timestamp_;
//acceleration noise components
float noise_ax;
float noise_ay;
};
#endif // TRACKING_H_
kalman_filter.h
#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_
#include "Dense"
using Eigen::MatrixXd;
using Eigen::VectorXd;
class KalmanFilter {
public:
/**
* Constructor
*/
KalmanFilter();
/**
* Destructor
*/
virtual ~KalmanFilter();
/**
* Predict Predicts the state and the state covariance
* using the process model
*/
void Predict();
/**
* Updates the state and
* @param z The measurement at k+1
*/
void Update(const VectorXd &z);
// state vector
VectorXd x_;
// state covariance matrix
MatrixXd P_;
// state transistion matrix
MatrixXd F_;
// process covariance matrix
MatrixXd Q_;
// measurement matrix
MatrixXd H_;
// measurement covariance matrix
MatrixXd R_;
};
#endif // KALMAN_FILTER_H_
kalman_filter.cpp
#include "kalman_filter.h"
KalmanFilter::KalmanFilter() {
}
KalmanFilter::~KalmanFilter() {
}
void KalmanFilter::Predict() {
x_ = F_ * x_;
MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;
}
void KalmanFilter::Update(const VectorXd &z) {
VectorXd z_pred = H_ * x_;
VectorXd y = z - z_pred;
MatrixXd Ht = H_.transpose();
MatrixXd S = H_ * P_ * Ht + R_;
MatrixXd Si = S.inverse();
MatrixXd PHt = P_ * Ht;
MatrixXd K = PHt * Si;
//new estimate
x_ = x_ + (K * y);
long x_size = x_.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}
measurement_package.h
#ifndef MEASUREMENT_PACKAGE_H_
#define MEASUREMENT_PACKAGE_H_
#include "Dense"
class MeasurementPackage {
public:
enum SensorType {
LASER, RADAR
} sensor_type_;
Eigen::VectorXd raw_measurements_;
int64_t timestamp_;
};
#endif // MEASUREMENT_PACKAGE_H_
main.cpp
#include <iostream>
#include <sstream>
#include <vector>
#include "Dense"
#include "measurement_package.h"
#include "tracking.h"
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::ifstream;
using std::istringstream;
using std::string;
using std::vector;
int main() {
/**
* Set Measurements
*/
vector<MeasurementPackage> measurement_pack_list;
// hardcoded input file with laser and radar measurements
string in_file_name_ = "obj_pose-laser-radar-synthetic-input.txt";
ifstream in_file(in_file_name_.c_str(), ifstream::in);
if (!in_file.is_open()) {
cout << "Cannot open input file: " << in_file_name_ << endl;
}
string line;
// set i to get only first 3 measurments
int i = 0;
while (getline(in_file, line) && (i<=3)) {
MeasurementPackage meas_package;
istringstream iss(line);
string sensor_type;
iss >> sensor_type; // reads first element from the current line
int64_t timestamp;
if (sensor_type.compare("L") == 0) { // laser measurement
// read measurements
meas_package.sensor_type_ = MeasurementPackage::LASER;
meas_package.raw_measurements_ = VectorXd(2);
float x;
float y;
iss >> x;
iss >> y;
meas_package.raw_measurements_ << x,y;
iss >> timestamp;
meas_package.timestamp_ = timestamp;
measurement_pack_list.push_back(meas_package);
} else if (sensor_type.compare("R") == 0) {
// Skip Radar measurements
continue;
}
++i;
}
// Create a Tracking instance
Tracking tracking;
// call the ProcessingMeasurement() function for each measurement
size_t N = measurement_pack_list.size();
// start filtering from the second frame
// (the speed is unknown in the first frame)
for (size_t k = 0; k < N; ++k) {
tracking.ProcessMeasurement(measurement_pack_list[k]);
}
if (in_file.is_open()) {
in_file.close();
}
return 0;
}
运行结果如下:
x_= 0.96749
0.405862
4.58427
-1.83232
P_= 0.0224541 0 0.204131 0
0 0.0224541 0 0.204131
0.204131 0 92.7797 0
0 0.204131 0 92.7797
x_= 0.958365
0.627631
0.110368
2.04304
P_= 0.0220006 0 0.210519 0
0 0.0220006 0 0.210519
0.210519 0 4.08801 0
0 0.210519 0 4.08801
x_= 1.34291
0.364408
2.32002
-0.722813
P_= 0.0185328 0 0.109639 0
0 0.0185328 0 0.109639
0.109639 0 1.10798 0
0 0.109639 0 1.10798