扩展卡尔曼滤波器

卡尔曼滤波算法包括以下几个步骤：

首次测量 - 滤波器首先会得到自行车相对于汽车位置的初始测量值，该数据来自雷达或激光雷达。
初始化状态空间或者协方差矩阵
t事件后，收到另外一次测量数据
预测 - 算法首先预测t时间后自行车的位置。通常会假定自行车速度为常量，x=vt进行计算
更新 - 滤波器将预测值和传感器的测量值进行比较。基于预测值和测量值共同给出更新后的位置。根据预测值和测量值的不确定性，给二者不同的权重。
随后，再次收到测量数据，进行下一次的预测和更新循环

预测

如果两个传感器的数据同时到达，则使用一个进行预测，另外一个进行测量更新

Kalman Filter Equations in C++

#include <iostream>
#include "Dense"
#include <vector>

using namespace std;
using namespace Eigen;

//Kalman Filter variables
VectorXd x;	// 状态向量
MatrixXd P;	// 协方差矩阵
VectorXd u;	// 运动控制向量
MatrixXd F;     // 状态转移矩阵
MatrixXd H;	// 测量矩阵
MatrixXd R;	// 测量协方差矩阵
MatrixXd I; 
MatrixXd Q;	// 处理协方差矩阵

vector<VectorXd> measurements;
void filter(VectorXd &x, MatrixXd &P);

int main() {
    // 变量初始化
	x = VectorXd(2);
	x << 0, 0;

	P = MatrixXd(2, 2);
	P << 1000, 0, 0, 1000;

	u = VectorXd(2);
	u << 0, 0;

	F = MatrixXd(2, 2);
	F << 1, 1, 0, 1;

	H = MatrixXd(1, 2);
	H << 1, 0;

	R = MatrixXd(1, 1);
	R << 1;

	I = MatrixXd::Identity(2, 2);

	Q = MatrixXd(2, 2);
	Q << 0, 0, 0, 0;

	// 构造一组测量值
	VectorXd single_meas(1);
	single_meas << 1;
	measurements.push_back(single_meas);
	single_meas << 2;
	measurements.push_back(single_meas);
	single_meas << 3;
	measurements.push_back(single_meas);

	//卡尔曼滤波
	filter(x, P);

	return 0;

}

// 卡尔曼滤波器
// 输入测量值向量，协方差矩阵
void filter(VectorXd &x, MatrixXd &P) {

	for (unsigned int n = 0; n < measurements.size(); ++n) {

		VectorXd z = measurements[n];
		/*
		 * KF Measurement update step
		 */
		VectorXd y = z - H * x;  //测量值更新（测量值-当前预测值）
		MatrixXd Ht = H.transpose();
		MatrixXd S = H * P * Ht + R;//估计误差+测量误差
		MatrixXd Si = S.inverse();
		MatrixXd K =  P * Ht * Si;//卡尔曼增益

		//new state
		x = x + (K * y); // 卡尔曼增益作为权重结合测量值计算新状态
		P = (I - K * H) * P;

		/*
		 * KF Prediction step
		 */
		x = F * x + u; // 新状态预测
		MatrixXd Ft = F.transpose();
		P = F * P * Ft + Q; //新协方差矩阵预测

		std::cout << "x=" << std::endl <<  x << std::endl;
		std::cout << "P=" << std::endl <<  P << std::endl;
	}
}

x=
0.999001
       0
P=
1001 1000
1000 1000
x=
   2.998
0.999002
P=
4.99002 2.99302
2.99302 1.99501
x=
3.99967
      1
P=
 2.33189 0.999168
0.999168 0.499501

状态估计

测量协方差矩阵

我们假定观测目标的速度在我们观察的时间内是恒定的，但是实际上并不是而且我们无法得知加速度大小，所以我们将加速度产生的位移作为误差添加到方程中。

其中，误差v表示如下：

我们用正态分布来表示它，即$ \nu \sim N(0,Q)$，该误差符合正态分布切均值为0，协方差为Q。

进而我们可以将加速度分离出来

协方差Q表示为： $Q = E[\nu \nu^T] = E[Gaa^TG^T] $

the expectation of ax times ax, which is the variance of ax:$\sigma_{ax}^2$
the expectation of ay times ay, which is the variance of ay: $\sigma_{ay}^2σ $
and the expectation of ax times ay, which is the covariance of $ax$ and $ay$: $\sigma_{axy}$ 如果我们加上x，y两个方向上的加速度不相关，则Q变换为：

激光雷达数据

激光雷达采集到点云，检测物体并得到物体的位置，而没有速度信息。因此我们的测量值Z是一个二维向量:

则y=Z-H*x中H为：

测量误差R：它表示从激光雷达引入的位置测量误差，一般是传感器本身的属性，由供应商提供。

#include "kalman_filter.h"

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_;
}

雷达数据

雷达可以测量目标的位置和速度，但是位置精度不高。此外，雷达测量数据是基于极坐标系的，因此使用h(x)将数据转换到直角系中。

测量值$ y = z - h(x^')$

h(x)是非线性的

泰勒公式

如果测量函数为非线性，则误差分布不满足正态分布我们可以通过求函数在某个点处的泰勒展开式，将其线性化。

函数在某点的一阶泰勒公式表示为：

$f(x)≈f(μ)+ \frac{∂x}{∂f(μ)}(x−μ)$

多元泰勒公式表示为：

雅可比矩阵

上面公式中，一阶的系数Df(a)叫做雅可比矩阵，我们忽略二阶及高阶项。

代码实现

MatrixXd CalculateJacobian(const VectorXd& x_state) {

	MatrixXd Hj(3,4);
	//recover state parameters
	float px = x_state(0);
	float py = x_state(1);
	float vx = x_state(2);
	float vy = x_state(3);

	//pre-compute a set of terms to avoid repeated calculation
	float c1 = px*px+py*py;
	float c2 = sqrt(c1);
	float c3 = (c1*c2);

	//check division by zero
	if(fabs(c1) < 0.0001){
		cout << "CalculateJacobian () - Error - Division by Zero" << endl;
		return Hj;
	}

	//compute the Jacobian matrix
	Hj << (px/c2), (py/c2), 0, 0,
		  -(py/c1), (px/c1), 0, 0,
		  py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2;

	return Hj;
}