基于PI自适应卡尔曼滤波的小车状态估计算法（含代码）

0 前言

在上一篇文章中，我在基于视觉的无人机移动平台自主降落的实验场景中，对小车的位置使用卡尔曼滤波和扩展卡尔曼滤波进行状态估计，里面包含了详细的建模过程和C++代码，请先看看那篇文章，跳转链接：
小车直线运动的C++卡尔曼滤波，以及如何升级为扩展卡尔曼滤波

1 PI自适应卡尔曼滤波理论公式

传统卡尔曼滤波器采用固定的测量噪声协方差矩阵R，不能充分体现传感器的噪声特性，可能会导致滤波精度下降甚至发散。卡尔曼滤波器正常工作时，残差方差的实际值与理论值应当非常接近，如果二者相差较大则说明传感器的噪声特性发生了变化，需对测量噪声协方差矩阵进行调整，使二者保持一致。对此，设计的PI自适应卡尔曼滤波算法如下。

对计算卡尔曼增益的公式（公式在上一篇博文）进行修改，用时变观测噪声协方差矩阵$R_k$代替原来的$R$，则卡尔曼增益$K_k$的计算公式变为：
$$K_k=\frac{P_{k/k-1}{H}^T}{H{P}_{k/k-1}{H}^T+R_k}$$

其中，$R_k=(1+{\alpha }_k)R_{k-1}$ 为第 k 次迭代时测量噪声协方差矩阵的估计值，${\alpha }_k$为测量噪声协方差矩阵的调整系数，也是整个 PI 自适应卡尔曼滤波算法的核心。

定义残差方差的理论值为：
$$D_k=H{P}_{k/k-1}{H}^T+R_{k-1}$$

定义残差方差的实际值为：
$$C_k=(Z_k-H_k{\hat{X}}_{k/k-1})(Z_k-H_k{\hat{X}}_{k/k-1}{)}^T$$

残差方差理论值与实际值之间的差值可以用两个矩阵的迹之差来表示，即
$$E_k=trace(D_k)-trace(C_k)$$
其中trace()表示求矩阵的迹。

PI自适应卡尔曼滤波算法的关键就在于求解调整系数${\alpha }_k$。由公式可以看出，残差方差理论值$D_k$是随测量噪声协方差矩阵$R_k$单调递增的。因此，通过调节$R_k$就可以改变$D_k$，从而使得$E_k$维持在0附近。于是，这里引入经典控制理论的PI控制算法，将$R_k$作为控制变量来计算调整系数${\alpha }_k$，表达式为：
$${\alpha }_k=K_p{E}_k+\beta K_i\sum _{n=0}^k{E}_n\Delta t$$

其中，$K_p$为比例系数，用于快速消除误差，$K_i$为积分系数，用于消除静态误差。而且，该 PI 控制算法在传统算法的基础上引入了积分分离，防止积分饱和，$\beta$为积分开关系数，表达式为：
$$\beta =\{\begin{array}{cc}1& |E_k\le \epsilon |\\ & \\ 0& |E_k>\epsilon |\end{array}$$

其中，$\epsilon$为开关阈值，可以根据实际情况选定。当误差大于阈值时，不使用积分环节，防止饱和; 当误差小于阈值时，积分环节开始介入，消除余差。

以上就是 PI 自适应卡尔曼滤波算法的理论部分，公式不多也不复杂，看上去很简洁，但是我在实际运用时却遇到了不少问题，下面就讲讲代码部分。

2 代码部分

代码部分是在标准KF的基础上改进而来的，所以最好还是先看看我上一篇文章~

2.1 C++代码

首先是数据的初始化：

	// AKF相关
	Eigen::MatrixXd X_AKF(4, 1); // 状态变量，包括x，y方向位置和速度
	Eigen::MatrixXd P_AKF(4, 4); // 先验估计协方差矩阵，后续会迭代收敛
	Eigen::MatrixXd F_AKF(4, 4); // 状态转移矩阵
	Eigen::MatrixXd H_AKF(2, 4); // 观测矩阵
	Eigen::MatrixXd R_AKF(2, 2); // 噪声协方差矩阵
	Eigen::MatrixXd G_AKF(4, 1); // 创建过程噪声协方差矩阵
	Eigen::MatrixXd Q_AKF(4, 4); // 计算过程噪声协方差矩阵（即Px,Py,Vx,Vy协方差矩阵）
	double car_pos_x_AKF, car_pos_y_AKF;
	double err_x_cm_AKF, err_y_cm_AKF;
	bool is_AKF_Initialized = 0; // 用于数据初始化
	double alpha = 0.0f;
	bool beta = 0;
	double En = 0.0f;

    // AKF部分数据初始化
    X_AKF << 0.0, 0.0, 0.0, 0.0;
    P_AKF << 10.0, 0, 0, 0, // 对位置的信任度更高
             0, 10.0, 0, 0, // 对位置的信任度更高
             0, 0, 10.0, 0, // 对速度的信任度更低
             0, 0, 0, 10.0; // 对速度的信任度更低
    F_AKF << 1.0, 0.0, dT, 0.0,
             0.0, 1.0, 0.0, dT,
             0.0, 0.0, 1.0, 0.0,
             0.0, 0.0, 0.0, 1.0;
    H_AKF << 1.0, 0.0, 0.0, 0.0,
             0.0, 1.0, 0.0, 0.0;
    R_AKF << 10.0, 0.0, 
             0.0, 10.0; // 可以根据实际效果来调
    Q_AKF = Eigen::MatrixXd::Identity(4, 4); // 对于简单的模型，工程上一般设为单位阵

数据初始化写好后，就是直接套公式了：

   /*********************AKF begin***********************/
   // 用第一个时刻的数据来初始化状态X
   if (is_AKF_Initialized == 0) {
       X_AKF << P_w(0, 0), P_w(1, 0), 0.0, 0.0;
       is_AKF_Initialized = 1;
   }

   // 预测模型
   X_AKF = F_AKF * X_AKF; // 预测未来状态（先验估计）
   P_AKF = F_AKF * P_AKF * F_AKF.transpose() + Q_AKF; // 预测误差协方差矩阵（先验估计协方差）

   // 观测方程
   Eigen::MatrixXd Z_AKF(2, 1);
   Z_AKF << P_w(0, 0), P_w(1, 0); // 没有补偿模型中心误差
   Eigen::MatrixXd y_AKF(2, 1);
   y_AKF = Z_AKF - (H_AKF * X_AKF);

   //计算调整系数alpha，AKF的关键
   Eigen::MatrixXd D_AKF(2, 2); // 残差方差理论值
   D_AKF = H_AKF * P_AKF * H_AKF.transpose() + R_AKF;
   cout << "D_AKF: " << D_AKF << endl;
   Eigen::MatrixXd C_AKF(2, 2); // 残差方差实际值
   C_AKF = y_AKF * y_AKF.transpose();
   cout << "C_AKF: " << C_AKF << endl;
   double Ek = D_AKF.trace() - C_AKF.trace(); // 残差方差误差
   Ek_vec.push_back(Ek); // 把Ek单独导出来方便调参
   cout << "Ek: " << Ek << endl;
   En += Ek;
   double Kp_AKF = 0.0001, Ki_AKF = 0.00001, var = 10.0;
   if (Ek <= var) beta = 1;
   else beta = 0; // 积分分离
   alpha = Kp_AKF * Ek + beta * Ki_AKF * En * dT;
   R_AKF = (1 - alpha) * R_AKF; // 对R进行实时估计并修正，PIAKF核心！这里好像要用负号不然就发散？？

   // 测量更新，计算卡尔曼增益
   Eigen::MatrixXd S_AKF(2, 4);
   S_AKF = H_AKF * P_AKF * H_AKF.transpose() + R_AKF; // 注意此时的R已经经过了修正
   Eigen::MatrixXd K_AKF(4, 2);
   K_AKF = (P_AKF * H_AKF.transpose()) * pseudoInverse(S_AKF);

   // 更新最优估计值
   X_AKF = X_AKF + (K_AKF * y_AKF); // 最优估计值
   cout << "X_AKF: " << X_AKF << endl; // 查看结果

   // 更新协方差矩阵
   // Eigen::MatrixXd I_AKF = Eigen::MatrixXd::Identity(4, 4);
   P_AKF = (I - (K_AKF * H_AKF)) * P_AKF;

   /*********************AKF end***********************/

下面展示一些效果图
先来看看不使用AKF，单独查看 Ek 的值的情况：

可以看到，在无人机的降落过程中，基于视觉检测小车位置的测量噪声特性会发生很大的变化，因此引入自适应卡尔曼滤波算法对测量噪声协方差矩阵实时修正是很有必要的。

而使用了 PIAKF 之后的大致效果如下图所示，Ek 可以逐渐收敛到0左右，但其实总体效果没感觉有多好，跟标准KF没差多少，后面会讲讲我遇到的问题。

2.2 python代码

我同时也写了一份python的纯仿真代码，也就是单纯为了验证算法效果的那种仿真，效果如下图所示。可以看到AKF（红色线）更接近真实值（蓝色线），比标准KF（绿色线）的误差更小，且随着时间推移误差会逐渐减小。
代码下载链接：
https://download.csdn.net/download/qq_42683787/88676806

3 一些后话

由于目前网上关于这个PI自适应卡尔曼滤波算法的开源资料比较少，我的代码也是从别人的论文里复现出来的，因此我在实际使用中遇到了不少的问题。
1、首先第一个问题是PI控制器的参数特别难调。上面代码中我设定的参数是

double Kp_AKF = 0.0001, Ki_AKF = 0.00001, var = 10.0;

Kp和Ki只要稍微大一点就会导致Ek发散，而且是一下子飙到无穷大的那种，我调参过程中完全看不到正常PI控制器调参的那种过渡过程，不知道为什么会产生这样的现象。。

2、算法其实并没有明显的效果。这可能是因为我这个场景本来就不太需要这种算法，也可能是我的传感器本身的噪声特性就比较稳定。但是话又说回来，其实搞这种花里胡哨的算法主要为了写论文而已，而且数据是可以做出来的，像上面2.2节的效果图，看着好像PIAKF的效果很好是吧？但其实是调参调出来的，拿标KF调一下Q和R也可以达到这样的效果。我记得一位老教授说过：不管你算法搞得多么复杂，到了实际项目中，肯定是要从最简单的算法开始做起。现在我对这句话是深有体会。

总结一下，我也只是个新手小白，对于KF和PIAKF算法也只是处于摸索阶段，其中有些内容可能会有错误，写这篇贴子更多是为了抛砖引玉，欢迎各位大佬进行批评指正~~