"录诗误字,啮臂流血"
前置事项: 原理部分大佬讲的很详细原理。
0. 简单认识
想象一个场景:实际场景中,假设有一个小车在做匀速直线运动,你能读到他的速度和位置。
信息1: 虽然但是实际中读到的速度和位置传感器的数值带有误差;
信息2:下一秒的位置理论上可以通过
s
=
s
p
r
e
+
v
Δ
t
s = s_{pre}+v\Delta t
s=spre+vΔt 计算出来,但是实际中肯定不会这么完美。
如何充分利用这两个信息,以得到尽可能准确的速度和位置信息?
如何充分利用这两个信息,以得到尽可能准确的速度和位置信息?
如何充分利用这两个信息,以得到尽可能准确的速度和位置信息?
卡尔曼滤波出来了,信息1我们称作观测信息;信息2称作预测的信息。通过给这两个信息权重计算出最终信息。
此之(#`O′)—卡尔曼滤波
1. 公式解读
(这一部分用来劝退,不能看懂记住如下5个公式就好,其实很简单)
有人说5个公式,有人说7个,实际上是一样的,只是7个的为了简化将一部分重新命名了而已。这里以5个为例,上边两个是预测,下边三个是更新
- 根据上一时刻信息预测此时的信息(): x ^ k ˉ = A k x ^ k − 1 + B k u k \hat{x}_{\bar{k}} = A_k\hat{x}_{{k-1}} + B_ku_k x^kˉ=Akx^k−1+Bkuk
- 根据上一刻信息预测此刻该有的协方差矩阵: P k ˉ = A k P k − 1 A k T + Q P_{\bar k} = A_k P_{k-1} A_k^T + Q Pkˉ=AkPk−1AkT+Q
- 求卡尔曼增益: K t = P k ˉ H k T H k P k ˉ H k T + R K_t = \frac{P_{\bar k} H^T_k} {H_k P_{\bar k} H_k^T + R} Kt=HkPkˉHkT+RPkˉHkT
- 用观测数据更新在步骤1中得到状态: x ^ k = x ^ k ˉ + K t ( z k − H k x ^ k ˉ ) \hat x_k = \hat x_{\bar k} + K_t(z_k - H_k \hat x _{\bar k}) x^k=x^kˉ+Kt(zk−Hkx^kˉ)
- 用卡尔曼增益更新步骤2中得到的协方差矩阵: P k = ( I − K t H k ) P k ˉ P_k = (I - K_t H_k) P_{\bar k} Pk=(I−KtHk)Pkˉ
解释:
步骤1中: A k A_k Ak 称为状态转移矩阵, B k B_k Bk 叫做控制矩阵。相应的 x ^ k − 1 \hat{x}_{{k-1}} x^k−1是上一时刻的状态,而 u k u_k uk 是控制输入,实际上这里用 u k − 1 u_{k-1} uk−1较为妥当;
;
步骤2中: P P P 是协方差矩阵,就是状态向量x中的向量两两作协方差,通俗的讲就是用来衡量系统不确定程度。表示系统对状态转移矩阵中某个量更可信则对应的数值低,不可信则大,一开始会比较大,但数据越累越多该值越来越低; Q Q Q 是无法用状态转移矩阵衡量的误差,称为过程噪声
;
步骤3中, H H H 是测量矩阵,它的意义,比如我们状态向量中设置的是速度即 [ v x , v y ] [v_x, v_y] [vx,vy], 但是我们测量是用不同传感器测量的,如果用陀螺仪则我们得到的是三个轴的速度 [ v x , v y , v z ] [v_x, v_y, v_z] [vx,vy,vz] ,为了和状态向量对齐,此时我们的测量矩阵应该是 [ 1 , 1 , 0 ] [1,1,0] [1,1,0],说白了就是实际和理论对齐的东西;这里的 R R R 是测量噪声矩阵,表示测量值和真实值的误差,一般厂家会给定。
;
步骤4,再更新状态量,新的状态量由预测和观测和权值(卡尔曼增益 K t K_t Kt)共同决定;
;
步骤5,重新更新系统不确定度,新的系统不确定度随着数据的输入而变小。
2. 应用举例
假设场景:
- 一块静止的单线激光雷达暴露在室内,室内有一个在运动的物体。现在可以拿到激光雷达数据 [ x , y ] [x , y] [x,y], 该物体的x 和 y 坐标,由于传感器误差,测得的值有一定误差;此外物体做匀速运动,但是由于地面摩擦存在,它的运动也非绝对匀速运动。现在要比较精确的求物体的坐标 [ x r e , y r e ] [x_{re}, y_{re}] [xre,yre]。
下边利用以上5个公式和C++实现用KF来求解这个问题。
3.解决方案(公式–代码对应 )
首先写出系统的观测量,根据题设,可以观测到的量如下:
z
=
[
x
y
]
z = \begin{bmatrix}x \\y \end{bmatrix}
z=[xy]
写出系统的观测量,为了更好利用所有以上信息,设状态量如下:
x
=
[
x
y
v
x
v
y
]
x = \begin{bmatrix}x \\y \\v_x \\v_y \end{bmatrix}
x=
xyvxvy
3.1 初始化
xXd表示非定长,为了其他函数也能调用。
class KalmanFilter
{
public:
KalmanFilter(){
is_initialized_ = false ;
};
~KalmanFilter();
void Initialization(Eigen::VectorXd x_in){
x_ = x_in ;
}
bool IsInitialized(){
return this->is_initialized_ ;
}
private:
bool is_initialized_ ; // flag of initialized
Eigen::VectorXd x_ ; // state vector
}
3.2 卡尔曼滤波过程
3.2.1. 预测—状态量
该物体的理想状态向量应该由速度和位置组成,不妨设理想的计算值为
x
=
[
x
,
y
,
v
x
,
v
y
]
T
x = [x, y, v_x, v_y]^T
x=[x,y,vx,vy]T
而对于匀速运动模型有:
x
^
k
ˉ
=
[
x
^
k
−
1
+
v
x
^
k
−
1
⋅
Δ
t
y
^
k
−
1
+
v
y
^
k
−
1
⋅
Δ
t
v
x
^
k
−
1
v
y
^
k
−
1
]
\hat{x}_{\bar{k}} = \begin{bmatrix} \hat{x}_{{k-1}} + v_{\hat{x}_{{k-1}}} \cdot \Delta t \\ \hat{y}_{{k-1}} + v_{\hat{y}_{{k-1}}} \cdot \Delta t \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix}
x^kˉ=
x^k−1+vx^k−1⋅Δty^k−1+vy^k−1⋅Δtvx^k−1vy^k−1
匀速运动 :
u
=
0
u = 0
u=0, 这里的u代表的加速减速作用
故按照上述推理,根据 x ^ k ˉ = A k x ^ k − 1 + B k u k \hat{x}_{\bar{k}} = A_k\hat{x}_{{k-1}} + B_ku_k x^kˉ=Akx^k−1+Bkuk进行展开,有
[ x ^ k − 1 + v x ^ k − 1 ⋅ Δ t y ^ k − 1 + v y ^ k − 1 ⋅ Δ t v x ^ k − 1 v y ^ k − 1 ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] ⋅ [ x ^ k − 1 y ^ k − 1 v x ^ k − 1 v y ^ k − 1 ] + [ 0 0 0 0 ] \begin{bmatrix} \hat{x}_{{k-1}} + v_{\hat{x}_{{k-1}}} \cdot \Delta t \\ \hat{y}_{{k-1}} + v_{\hat{y}_{{k-1}}} \cdot \Delta t \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} \hat{x}_{{k-1}} \\ \hat{y}_{{k-1}} \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} x^k−1+vx^k−1⋅Δty^k−1+vy^k−1⋅Δtvx^k−1vy^k−1 = 10000100Δt0100Δt01 ⋅ x^k−1y^k−1vx^k−1vy^k−1 + 0000
所以这里的状态转移矩阵 F F F 为上边第二个矩阵,但是由于 Δ t \Delta t Δt 大小不定,因此手动输入:
// state transistion matrix for predict x'
void SetF(Eigen::MatrixXd F_in){
F_ = F_in ;
}
3.2.2 预测—系统协方差
根据
P
k
ˉ
=
A
k
P
k
−
1
A
k
T
+
Q
P_{\bar k} = A_k P_{k-1} A_k^T + Q
Pkˉ=AkPk−1AkT+Q
这里的
P
P
P 是系统的不确定度,我们使用的是激光雷达,由于要运算的状态量是
[
x
,
y
,
v
x
,
v
y
]
T
[x, y, v_x, v_y]^T
[x,y,vx,vy]T, 对激光雷达系统而言,对位置的信任度(确定度)显然较高,而激光雷达不具有测量速度能力,对速度确定度低。因此可以设置P为(后边会不断更新):
P
=
[
1
0
0
0
0
1
0
0
0
0
100
0
0
0
0
100
]
P = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 100 & 0 \\ 0 & 0 & 0 & 100 \end{bmatrix}
P=
10000100001000000100
Q
Q
Q 是这里的过程噪声,不能用状态转移矩阵表示,比如由于该物体运动时地面摩擦导致的随机噪声,工程上一般设置为单位阵。
Q
=
[
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
]
Q = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
Q=
1000010000100001
可以写出本部分代码:
/**********calculate / predict x'************/
// state transistion matrix for predict x'
void SetF(Eigen::MatrixXd F_in){
F_ = F_in ;
}
/**********calculate / predict P'************/
// state covariance matrix for predict x'
void SetP(Eigen::MatrixXd P_in){
P_ = P_in ;
}
// process covariance matrix for predict x'
void SetQ(Eigen::MatrixXd Q_in){
Q_ = Q_in ;
}
void Prediction(){
x_ = F_ * x_ ;
Eigen::MatrixXd Ft = F_.transpose() ;
P_ = F_ * P_ * Ft + Q_ ;
}
private:
bool is_initialized_ ; // flag of initialized
Eigen::VectorXd x_ ; // state vector
Eigen::MatrixXd F_ ; // state transistion matrix
Eigen::MatrixXd P_ ; // state covariance matrix
Eigen::MatrixXd Q_ ; // process covariance matrix
3.2.3 更新—H测量矩阵
上边卡尔曼第4个公式,中有一段:
z k − H k x ^ k ˉ z_k - H_k \hat x _{\bar k} zk−Hkx^kˉ
这里的
z
k
z_k
zk 实际上是测量值
z
k
=
[
x
m
k
,
y
m
k
]
T
z_k = [x_{mk}, y_{mk}]^T
zk=[xmk,ymk]T, 而
x
k
ˉ
=
[
x
,
y
,
v
x
,
v
y
]
T
x _{\bar k} = [x, y, v_x, v_y]^T
xkˉ=[x,y,vx,vy]T, 为了使测量数据能够和状态量进行运算,所以引入了
H
H
H 矩阵,也称测量矩阵。显然这里应该给:
H
=
[
1
0
0
0
0
1
0
0
]
H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix}
H=[10010000]
只有这样才能计算
[
x
m
k
y
m
k
]
−
[
1
0
0
0
0
1
0
0
]
[
x
^
k
−
1
+
v
x
^
k
−
1
⋅
Δ
t
y
^
k
−
1
+
v
y
^
k
−
1
⋅
Δ
t
v
x
^
k
−
1
v
y
^
k
−
1
]
\begin{bmatrix} x_{mk} \\ y_{mk}\end{bmatrix} - \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix}\begin{bmatrix} \hat{x}_{{k-1}} + v_{\hat{x}_{{k-1}}} \cdot \Delta t \\ \hat{y}_{{k-1}} + v_{\hat{y}_{{k-1}}} \cdot \Delta t \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix}
[xmkymk]−[10010000]
x^k−1+vx^k−1⋅Δty^k−1+vy^k−1⋅Δtvx^k−1vy^k−1
3.2.3 更新—卡尔曼增益K
根据:
K
t
=
P
k
ˉ
H
k
T
H
k
P
k
ˉ
H
k
T
+
R
K_t = \frac{P_{\bar k} H^T_k} {H_k P_{\bar k} H_k^T + R}
Kt=HkPkˉHkT+RPkˉHkT
这里还差一个
R
R
R 矩阵未知, 它实际上是这里激光雷达的测量噪声矩阵,表示测量值和真实值之间的差值,一般情况,传感器的厂家会提供该值,比如我这里的:
R
=
[
0.225
0
0
0.225
]
R = \begin{bmatrix} 0.225 & 0 \\ 0 & 0.225 \end{bmatrix}
R=[0.225000.225]
卡尔曼增益可得
3.2.4 再更新状态量和系统协方差矩阵
x ^ k = x ^ k ˉ + K t ( z k − H k x ^ k ˉ ) P k = ( I − K t H k ) P k ˉ \hat x_k = \hat x_{\bar k} + K_t(z_k - H_k \hat x _{\bar k}) \\ \;\\ P_k = (I - K_t H_k) P_{\bar k} x^k=x^kˉ+Kt(zk−Hkx^kˉ)Pk=(I−KtHk)Pkˉ
这里再根据卡尔曼增益和测量值更新一下预测值就行了。
以上三节的代码如下
void MeasurementUpdate(const Eigen::VectorXd &z){
Eigen::VectorXd y = z - H_ * x_ ;
Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_ ;
Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse() ;
x_ = x_ + (K * y) ;
int size = x_.size() ;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size,size) ;
P_ = (I - K*H_) * P_ ;
}
至此,我们的卡尔曼滤波函数就写好了。最终在main函数中调用即可。
4. 源码
4.1 KalmanFilter.hpp 库
#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H
#include "Eigen/Dense"
class KalmanFilter
{
public:
KalmanFilter(){
is_initialized_ = false ;
};
~KalmanFilter();
void Initialization(Eigen::VectorXd x_in){
x_ = x_in ;
}
bool IsInitialized(){
return this->is_initialized_ ;
}
/**********calculate / predict x'************/
// state transistion matrix for predict x'
void SetF(Eigen::MatrixXd F_in){
F_ = F_in ;
}
/**********calculate / predict P'************/
// state covariance matrix for predict x'
void SetP(Eigen::MatrixXd P_in){
P_ = P_in ;
}
// process covariance matrix for predict x'
void SetQ(Eigen::MatrixXd Q_in){
Q_ = Q_in ;
}
void SetH(Eigen::MatrixXd H_in){
H_ = H_in ;
}
void SetR(Eigen::MatrixXd R_in){
R_ = R_in ;
}
void Prediction(){
x_ = F_ * x_ ;
Eigen::MatrixXd Ft = F_.transpose() ;
P_ = F_ * P_ * Ft + Q_ ;
}
void MeasurementUpdate(const Eigen::VectorXd &z){
Eigen::VectorXd y = z - H_ * x_ ;
Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_ ;
Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse() ;
x_ = x_ + (K * y) ;
int size = x_.size() ;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size,size) ;
P_ = (I - K*H_) * P_ ;
}
Eigen::VectorXd GetX(){
return x_ ;
}
private:
bool is_initialized_ ; // flag of initialized
Eigen::VectorXd x_ ; // state vector
Eigen::MatrixXd F_ ; // state transistion matrix
Eigen::MatrixXd P_ ; // state covariance matrix
Eigen::MatrixXd Q_ ; // process covariance matrix
Eigen::MatrixXd H_ ; // mesurement matrix
Eigen::MatrixXd R_ ; // mesurement covariance matrix
};
KalmanFilter::~KalmanFilter()
{
}
#endif
4.2 main.cpp 使用卡尔曼滤波
#include "kalman_filter.hpp"
#include <iostream>
#include <fstream>
#include <vector>
namespace plt = matplotlibcpp;
//read the data be created in ./data_script
bool GetLidarData(double& x, double& y, double& time){
static bool is_read_file_finished = false ;
static std::vector<double> file_x, file_y, file_time;
static std::string line, value;
if(!is_read_file_finished){
is_read_file_finished = true ;
std::string filename = "../data_script/file.txt";
std::ifstream file(filename);
while (std::getline(file, line)) {
std::stringstream ss(line);
std::getline(ss, value, ',');
file_x.push_back(std::stod(value));
std::getline(ss, value, ',');
file_y.push_back(std::stod(value));
std::getline(ss, value, ',');
file_time.push_back(std::stod(value));
}
}
if (file_time.size()>0){
x = file_x.front() ;
file_x.erase(file_x.begin()) ;
y = file_y.front() ;
file_y.erase(file_y.begin()) ;
time = file_time.front() ;
file_time.erase(file_time.begin()) ;
return true ;
} else {
return false ;
}
}
int main(int argc, const char** argv) {
double m_x = 0.0, m_y = 0.0 ;
double last_timestamp = 0.0, now_timestamp = 0.0 ;
KalmanFilter KF ;
while (GetLidarData(m_x, m_y, now_timestamp))
// Initialize kalman filter
{
// Initialize P,Q,H,R
if (!KF.IsInitialized())
{
last_timestamp = now_timestamp ;
Eigen::VectorXd x_in(4,1) ;
x_in << m_x, m_y, 0.0, 0.0 ;
KF.Initialization(x_in) ;
// state convarince matrix
Eigen::MatrixXd P_in(4, 4) ;
P_in << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 100.0, 0.0,
0.0, 0.0, 0.0, 100.0 ;
KF.SetP(P_in) ;
// process convariance matrix
Eigen::MatrixXd Q_in(4, 4) ;
Q_in << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0 ;
KF.SetQ(Q_in) ;
// measurement matrix
Eigen::MatrixXd H_in(2, 4) ;
H_in << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0 ;
KF.SetH(H_in) ;
// measurement convarince matrix
//R is provided by Sensor producer, in their datasheet
Eigen::MatrixXd R_in(2, 2) ;
R_in << 0.0225, 0.0,
0.0, 0.0225 ;
KF.SetR(R_in) ;
}
//state transition matrix
double delta_t = now_timestamp - last_timestamp ;
last_timestamp = now_timestamp ;
Eigen::MatrixXd F_in(4, 4) ;
F_in << 1.0, 0.0, delta_t, 0.0,
0.0, 1.0, 0.0, delta_t,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0 ;
KF.SetF(F_in) ;
KF.Prediction() ;
// measurement value
Eigen::VectorXd z(2, 1) ;
z << m_x, m_y ;
KF.MeasurementUpdate(z) ;
//result
Eigen::VectorXd x_out = KF.GetX() ;
std::cout << "original output x : " << m_x <<
" y: " << m_y << std::endl ;
std::cout << "kalman output x : " << x_out(0) <<
" y: " << x_out(1) << std::endl ;
}
return 0;
}
这里激光雷达存放在file.txt文件中,数据格式是 x, y, time ,逐行
4.3 CMakeLists.txt文件构建cmake工程
# CMakeLists.txt
# 设置构建类型为 Release
cmake_minimum_required(VERSION 3.10)
project(common_kf)
set(CMAKE_CXX_STANDARD 14) # 使用 C++14 标准
# 添加当前目录到 include 路径
include_directories(${PROJECT_SOURCE_DIR}/include)
# 添加源文件到编译
add_executable(KalDemo main.cpp)