卡尔曼滤波算法应用

Kalman算法简介

1.卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
2.由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
3.卡尔曼滤波算法是对贝叶斯滤波的一种具体实现。
4.卡尔曼滤波算法本质就是利用两个正态分布的融合仍是正态分布这一特性进行迭代而已。
5.卡尔曼滤波算法其实就是把模型算出来的值和传感器的观测值进行加权平均，不断迭代的过程。

Kalman算法应用场景

Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。
在ADAS领域中常见的车道线跟踪、目标跟踪，雷达跟踪等算法中也离不开卡尔曼滤波算法。

Kalman滤波和贝叶斯滤波的关系

总结：卡尔曼滤波算法是对贝叶斯滤波的一种具体实现。
其实，卡尔曼滤波(Kalman filters)、粒子滤波(Particle filters)、隐含马尔科夫模型(Hidden Markov models)、动态贝叶斯网络（Dynamic Bayesian networks）等算法都和贝叶斯滤波算法非常相似。

贝叶斯滤波——已知上一状态、观测值和控制命令来估计当前状态。

Kalman滤波计算步骤

Kalman计算公式和opencv对应关系

Kalman代码参数

Mat statePre;           //< 1.状态先验估计（预测状态）predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
Mat errorCovPre;        //< 2.先验估计协方差矩阵priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
Mat gain;               //< 3.卡尔曼增益矩阵Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
Mat statePost;          //< 4.状态后验估计（修正状态）corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
Mat errorCovPost;       //< 5.后验估计协方差矩阵posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

Mat transitionMatrix;   //< 状态转移矩阵state transition matrix (A)
Mat controlMatrix;      //< 控制矩阵control matrix (B) (not used if there is no control)
Mat measurementMatrix;  //< 测量矩阵measurement matrix (H)

Mat processNoiseCov;    //< 过程噪声协方差矩阵process noise covariance matrix (Q)
Mat measurementNoiseCov;//< 测量噪声协方差矩阵measurement noise covariance matrix (R)

Kalman代码步骤

1.初始化

// 构造kf对象
kf = cv::KalmanFilter(state_num, measure_num);
// 初始化参数
// 定义一个7*7转换矩阵
kf.transitionMatrix = (cv::Mat_<float>(state_num, state_num) <<
			1, 0, 0, 0, 1, 0, 0,
			0, 1, 0, 0, 0, 1, 0,
			0, 0, 1, 0, 0, 0, 1,
			0, 0, 0, 1, 0, 0, 0,
			0, 0, 0, 0, 1, 0, 0,
			0, 0, 0, 0, 0, 1, 0,
			0, 0, 0, 0, 0, 0, 1);

// 观测矩阵H
cv::setIdentity(kf.measurementMatrix);
// 过程噪声协方差矩阵Q
cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-5));
// 测量噪声协方差矩阵R
cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(1e-1));
// 估计协方差矩阵P
cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1));

// 定义初始观测值
measurement = cv::Mat::zeros(measure_num, 1, CV_32F);
// 定义系统初始状态
kf.statePost.at<float>(0, 0) = state_mat.x;
kf.statePost.at<float>(1, 0) = state_mat.y;
kf.statePost.at<float>(2, 0) = state_mat.w;
kf.statePost.at<float>(3, 0) = state_mat.h;

2.预测

kf.predict();

3.修正

// 观测矩阵
measurement.at<float>(0, 0) = state_mat.x;
measurement.at<float>(1, 0) = state_mat.y;
measurement.at<float>(2, 0) = state_mat.w;
measurement.at<float>(3, 0) = state_mat.h;

// 更新
kf.correct(measurement);

Kalman五大过程示意图

Kalman参数调整

1.过程噪声协方差矩阵Q（processNoiseCov，预测）
1）越小越容易收敛（太小容易发散），对模型预测的值信任度越高。
2）越大对测量值的信任度越高。
3）Q->0：只相信预测值；Q->无穷：只相信观测值。

2.测量噪声协方差矩阵R（measurementNoiseCov，观测）
1）越大卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低。
2）越小系统收敛越快，对新测量的值的信任度越高，但过小则容易出现震荡。

3.估计协方差矩阵P（errorCovPost，误差）
1）表示对当前预测状态的信任度，越小说明越信任当前的预测状态。
2）它的值决定了初始收敛速度，一般开始设较小的值便于获取较快的收敛速度。
3）随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，这个时候的Kg也是最优的，这个值只是影响初始收敛速度。

Kalman代码实现

# ---------状态初始化----------------------------------------
# 状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1, 0, 0, 0, 1, 0],
              [0, 1, 0, 0, 0, 1],
              [0, 0, 1, 0, 0, 0],
              [0, 0, 0, 1, 0, 0],
              [0, 0, 0, 0, 1, 0],
              [0, 0, 0, 0, 0, 1]])

# 状态观测矩阵
H = np.eye(A.shape[0])

# 过程噪声协方差矩阵Q，p(w)~N(0,Q)，噪声来自真实世界中的不确定性,
# 在跟踪任务当中，过程噪声来自于目标移动的不确定性（突然加速、减速、转弯等）
Q = np.eye(A.shape[0]) * 0.1

# 观测噪声协方差矩阵R，p(v)~N(0,R)
# 观测噪声来自于检测框丢失、重叠等
R = np.eye(H.shape[0]) * 1

# 状态估计协方差矩阵P初始化
P = np.eye(A.shape[0])

X_posterior = np.array(initial_state)   # （初始）后验估计状态
P_posterior = np.array(P)               # （初始）后验估计协方差
Z = np.array(initial_state)             # （初始）观测值

while True:
    # 1.状态先验估计[预测]------------
    X_prior = np.dot(A, X_posterior)

    # 2.先验估计协方差矩阵------------
    P_prior = np.dot(np.dot(A, P_posterior), A.T) + Q

    # 3.计算卡尔曼增益----------------
    k1 = np.dot(P_prior, H.T)
    k2 = np.dot(np.dot(H, P_prior), H.T) + R
    K = np.dot(k1, np.linalg.inv(k2))

    # 4.状态后验估计-----------------
    X_posterior_1 = Z - np.dot(H, X_prior)
    X_posterior = X_prior + np.dot(K, X_posterior_1)

    # 5.后验估计协方差矩阵------------
    P_posterior_1 = np.eye(6) - np.dot(K, H)
    P_posterior = np.dot(P_posterior_1, P_prior)