传感器融合算法：卡尔曼滤波器和扩展卡尔曼滤波器
传感器融合步骤
卡尔曼滤波器算法将执行以下步骤：
- 第一次测量 - 过滤器将接收自行车相对于汽车位置的初始测量值。这些测量将来自雷达或激光雷达传感器。
- 初始化状态和协方差矩阵 - 过滤器将根据第一次测量初始化自行车的位置。
- 然后汽车将在一段时间后接收另一个传感器测量值 Δ t \Delta{t} Δt。
- 预测 - 算法将预测自行车在一段时间后的位置 Δ t \Delta{t} Δt。预测自行车位置的一种基本方法 Δ t \Delta{t} Δt是假设自行车的速度是恒定的; 因此自行车将移动速度 Δ t \Delta{t} Δt。在扩展卡尔曼滤波器课程中，我们假设速度是恒定的; 在无损的卡尔曼滤波器课程中，我们将介绍一个更复杂的运动模型。
- 更新 - 过滤器将“预测”位置与传感器测量值进行比较。将预测的位置和测量的位置组合以给出更新的位置。卡尔曼滤波器将根据每个值的不确定性对预测位置或测量位置施加更多权重。
- 然后汽车将在一段时间后接收另一个传感器测量值 Δ t \Delta{t} Δt。然后该算法执行另一个预测和更新步骤。
1.使用激光雷达数据的基础卡尔曼滤波器
激光测量允许我们获取物体的当前位置而不是其速度。另一方面，激光测量具有更高的分辨率。
状态预测
协方差矩阵Q
2.扩展卡尔曼滤波器——使用雷达数据
雷达数据带来了一些更困难的挑战。雷达返回数据值的是基于极坐标系，其由三个部分构成:
- ρ /Range(从原点到此的距离)
- ϕ / bearing ( ρ 和 x的夹角),
- ρ˙：接近率/距离变化率
由于没有H矩阵将状态向量映射到雷达的测量空间，我们需要一个函数h（x）来将状态空间银蛇到测量空间以测量更新步骤。这个方法是映射极坐标到笛卡尔坐标的方法的衍生方法，定义如下：
这个映射阐述了一个非线性函数，它将使得卡尔曼滤波器的过程和测量符合高斯分布的假设无效。扩展卡尔曼滤波器使用局部线性模型来逼近非线性模型，然后使用卡尔曼滤波应用到逼近值上。局部线性逼近是通过计算当前状态估计的一阶泰勒展开得出的。一阶的逼近也叫雅克比矩阵。
扩展卡尔曼滤波
主要区别是：
- 在计算P ’ 时，F矩阵将被Fj代替。
- 当计算S，K和P时，卡尔曼滤波器中的H矩阵将被雅可比矩阵Hj代替。
- 为了计算x '，使用预测更新函数f代替F矩阵。
- 为了计算y，使用h函数代替H矩阵。
其中，非线性函数 f ( x , u ) ， h ( x ′ ) f(x,u)，h(x^′) f(x,u)，h(x′)用非线性得到了更精准的状态预测值、映射后的测量值；线性变换Fj，Hj通过线性变换使得变换后的x，z仍满足高斯分布的假设。
F j ， H j F_j，H_j Fj，Hj计算方式如下：
F j = ∂ f ( x , u ) x F_{j}=\frac{\partial f(x,u)}{x} Fj=x∂f(x,u)
H j = ∂ h ( x ′ ) x H_{j}=\frac{\partial h(x^{'})}{x} Hj=x∂h(x′)
均方根值误差
kalman_filter.h
#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_
#include "Eigen/Dense"
class KalmanFilter {
public:
// state vector
Eigen::VectorXd x_;
// state covariance matrix
Eigen::MatrixXd P_;
// state transition matrix
Eigen::MatrixXd F_;
// process covariance matrix
Eigen::MatrixXd Q_;
// measurement matrix
Eigen::MatrixXd H_;
// measurement covariance matrix
Eigen::MatrixXd R_;
/**
* Constructor
*/
KalmanFilter();
/**
* Destructor
*/
virtual ~KalmanFilter();
/**
* Init Initializes Kalman filter
* @param x_in Initial state
* @param P_in Initial state covariance
* @param F_in Transition matrix
* @param H_in Measurement matrix
* @param R_in Measurement covariance matrix
* @param Q_in Process covariance matrix
*/
void Init(Eigen::VectorXd &x_in, Eigen::MatrixXd &P_in, Eigen::MatrixXd &F_in,
Eigen::MatrixXd &H_in, Eigen::MatrixXd &R_in, Eigen::MatrixXd &Q_in);
/**
* Prediction Predicts the state and the state covariance
* using the process model
* @param delta_T Time between k and k+1 in s
*/
void Predict();
/**
* Updates the state by using standard Kalman Filter equations
* @param z The measurement at k+1
*/
void Update(const Eigen::VectorXd &z);
/**
* Updates the state by using Extended Kalman Filter equations
* @param z The measurement at k+1
*/
void UpdateEKF(const Eigen::VectorXd &z);
/**
* A helper method to normalize the difference angle.
*/
void NormalizeAngle(double& phi);
};
#endif /* KALMAN_FILTER_H_ */
FusionEKF.h
#include "kalman_filter.h"
#include "tools.h"
using Eigen::MatrixXd;
using Eigen::VectorXd;
KalmanFilter::KalmanFilter() {}
KalmanFilter::~KalmanFilter() {}
void KalmanFilter::Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,
MatrixXd &H_in, MatrixXd &R_in, MatrixXd &Q_in) {
x_ = x_in;
P_ = P_in;
F_ = F_in;
H_ = H_in;
R_ = R_in;
Q_ = Q_in;
}
void KalmanFilter::Predict() {
/**
TODO:
* predict the state
*/
x_ = F_ * x_;
MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;
}
void KalmanFilter::Update(const VectorXd &z) {
/**
TODO:
* update the state by using Kalman Filter equations
*/
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_;
}
void KalmanFilter::UpdateEKF(const VectorXd &z) {
/**
TODO:
* update the state by using Extended Kalman Filter equations
*/
float px = x_(0);
float py = x_(1);
float vx = x_(2);
float vy = x_(3);
float rho = sqrt(px*px + py*py);
float theta = atan2(py, px);
float rho_dot = (px*vx + py*vy) / rho;
VectorXd z_pred = VectorXd(3);
z_pred << rho, theta, rho_dot;
VectorXd y = z - z_pred;
NormalizeAngle(y(1));
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_;
}
void KalmanFilter::NormalizeAngle(double& phi) {
/**
TODO:
* Normalize the difference angle between -pi to pi.
*/
phi = atan2(sin(phi), cos(phi));
}
FusionEKF.cpp
#ifndef FusionEKF_H_
#define FusionEKF_H_
#include "measurement_package.h"
#include "Eigen/Dense"
#include #include "FusionEKF.h"
#include "tools.h"
#include "Eigen/Dense"
#include 代写！！！