14 卡尔曼滤波及代码实现

14 卡尔曼滤波及代码实现

14.0 基本概念

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

通俗来说就是，线性数学模型算出预测值+传感器测量值=更准确的测量值。根据数学模型，由第 $k$ 时刻的值递推得到第 $k+1$ 时刻的预测值，结合第 $k+1$ 时刻的观测值，得到第 $k+1$ 时刻更精准的值。

卡尔曼滤波主要用于 线性高斯系统。

14.1 公式推导

（1）线性高斯系统表达

状态方程：

$${\mathbf{x}}_k=\mathbf{A}{\mathbf{x}}_{k-1}+\mathbf{B}{\mathbf{u}}_k+{\mathbf{w}}_k$$

观测方程：

$${\mathbf{z}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{v}}_k$$

其中，${\mathbf{x}}_k$ 为状态量，${\mathbf{z}}_k$ 为观测量，$\mathbf{A}$ 为状态转移矩阵，${\mathbf{B}}_k$ 为控制输入矩阵，$\mathbf{H}$ 为状态观测矩阵。

${\mathbf{w}}_k$ 是过程噪声，服从高斯分布，${\mathbf{w}}_k$ 是观测噪声，也服从高斯分布，即：

$${\mathbf{w}}_k\sim N(0,\mathbf{Q})$$

$${\mathbf{v}}_k\sim N(0,\mathbf{R})$$

其中 $\mathbf{Q}$ 是过程噪声的协方差，$\mathbf{R}$ 是观测噪声的协方差。

卡尔曼滤波包括预测和更新两步。

（2）预测（先验）

预测是根据上一时刻的状态量，由状态方程预测出下一时刻的状态量 ${\hat{\mathbf{x}}}_k^{-}$ ，以及状态量误差协方差的先验估计矩阵 ${\mathbf{P}}_k^{-}$。这是没有加观测值的。

$${\hat{\mathbf{x}}}_k^{-}=\mathbf{A}{\hat{\mathbf{x}}}_{k-1}+\mathbf{B}{\mathbf{u}}_k$$

$${\mathbf{P}}_k^{-}={\mathbf{AP}}_{k-1}{\mathbf{A}}^T+\mathbf{Q}$$

其中，$\mathbf{A}{\hat{\mathbf{x}}}_{k-1}$ 是上一时刻的最优估计。

（3）更新（后验）

加入观测，对预测值进行更新校正，得到最优后验估计。

首先计算增益矩阵

$${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}^T(\mathbf{H}{\mathbf{P}}_k^{-}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}$$

更新状态量及其协方差矩阵

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-\mathbf{H}{\hat{\mathbf{x}}}_k^{-})$$

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}){\mathbf{P}}_k^{-}$$

14.2 代码实现

以雷达追踪目标为背景，系统的状态方程为

$${\left[\begin{array}{c}x\\ y\\ Vx\\ Vy\\ ax\\ ay\end{array}\right]}_{k+1}=\left[\begin{array}{cccccc}1& 0& {\delta }_t& 0& 0.5{\delta }_t^2& 0\\ 0& 1& 0& {\delta }_t& 0& 0.5{\delta }_t^2\\ 0& 0& 1& 0& {\delta }_t& 0\\ 1& 0& 0& 1& 0& {\delta }_t\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 1& 0& 1\end{array}\right]{\left[\begin{array}{c}x\\ y\\ Vx\\ Vy\\ ax\\ ay\end{array}\right]}_k$$

观测方程

$${\left[\begin{array}{c}x\\ y\end{array}\right]}_{k+1}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\end{array}\right]{\left[\begin{array}{c}x\\ y\\ Vx\\ Vy\\ ax\\ ay\end{array}\right]}_k$$

/***********************************************************                                          *
* Time: 2023/11/26
* Author: xiaocong
* Function: 卡尔曼滤波
***********************************************************/
#ifndef KALMANFILTER_H
#define KALMANFILTER_H#include <eigen3/Eigen/Dense>
#include <iostream>using namespace Eigen;
using namespace std;class KalmanFilter
{
public:KalmanFilter(int stateSize, int measSize, int uSize);               // 构造函数void init(VectorXd& x, MatrixXd& P, MatrixXd& R, MatrixXd& Q);      // 初始化void predict(MatrixXd& A);void predict(MatrixXd& A, MatrixXd& B, VectorXd& u);            // 重载，针对有控制输入的情况VectorXd update(MatrixXd& H, VectorXd z_meas);                      // 更新~KalmanFilter();                                                    // 析构函数private:VectorXd x_;         // 状态变量VectorXd z_;         // 观测变量MatrixXd A_;         // 状态转移矩阵MatrixXd B_;         // 控制矩阵VectorXd u_;         // 控制变量MatrixXd P_;         // 状态值的协方差矩阵MatrixXd H_;         // 观测矩阵MatrixXd R_;         // 观测噪声协方差矩阵MatrixXd Q_;         // 过程噪声协方差矩阵
};#endif //KALMANFILTER_H

#include "../inlude/KalmanFilter.h"// 构造函数
KalmanFilter::KalmanFilter(int stateSize, int measSize, int uSize)
{if (stateSize == 0 || measSize == 0){std::cerr << "Error, State size and measurement size must bigger than 0" << endl;}x_.resize(stateSize);x_.setZero();A_.resize(stateSize, stateSize);A_.setIdentity();u_.resize(uSize);u_.setZero();B_.resize(stateSize, uSize);B_.setZero();P_.resize(stateSize, stateSize);P_.setIdentity();H_.resize(measSize, stateSize);H_.setZero();Q_.resize(stateSize, stateSize);Q_.setIdentity();R_.resize(measSize, measSize);R_.setIdentity();
}void KalmanFilter::init(VectorXd& x, MatrixXd& P, MatrixXd& R, MatrixXd& Q)
{x_ = x;P_ = P;R_ = R;Q_ = Q;
}void KalmanFilter::predict(MatrixXd& A)         // 没有控制输入u
{A_ = A;x_ = A * x_;P_ = A_ * P_ * A_.transpose() + Q_;
}void KalmanFilter::predict(MatrixXd& A, MatrixXd& B, VectorXd& u)       // 有控制输入u
{A_ = A;B_ = B;u_ = u;x_ = A * x_ + B * u_;P_ = A_ * P_ * A_.transpose() + Q_;
}VectorXd KalmanFilter::update(MatrixXd& H, VectorXd z_meas)      // 更新
{H_ = H;MatrixXd temp = H_ * P_ * H_.transpose() + R_;MatrixXd K = P_ * H_.transpose() * temp.inverse();x_ = x_ + K * (z_meas - H_ * x_);               // 更新 x_kMatrixXd I = MatrixXd::Identity(x_.rows(), x_.rows());P_ = (I - K * H_) * P_;return x_;
}KalmanFilter::~KalmanFilter()
{}

#include "../inlude/KalmanFilter.h"
#include <fstream>#define N 1000
#define T 0.01double data_x[N], data_y[N];// 模型函数
double sample(double x0, double v0, double acc, double t)
{return x0 + v0 * t + 0.5 * acc * t * t;
}double getRand()
{return 0.5 * rand() / RAND_MAX - 0.25;   //[-0.25, 0.25)
}int main()
{ofstream fout;fout.open("../data/data.txt");// 生成观测值double t;for (int i = 0; i < N; i++){t = T * i;data_x[i] = sample(0, -4.0, 0.1, t) + getRand();data_y[i] = sample(0.1, 2.0, 0, t) + getRand();}int stateSize = 6;int measSize = 2;int uSize = 0;KalmanFilter kf(stateSize, measSize, uSize);Eigen::MatrixXd A(stateSize, stateSize);A << 1, 0, T, 0, 1 / 2 * T * T, 0,0, 1, 0, T, 0, 1 / 2 * T * T,0, 0, 1, 0, T, 0,0, 0, 0, 1, 0, T,0, 0, 0, 0, 1, 0,0, 0, 0, 0, 0, 1;Eigen::MatrixXd B(0, 0);Eigen::MatrixXd H(measSize, stateSize);H << 1, 0, 0, 0, 0, 0,0, 1, 0, 0, 0, 0;Eigen::MatrixXd P(stateSize, stateSize);P.setIdentity();Eigen::MatrixXd R(measSize, measSize);R.setIdentity() * 0.01;Eigen::MatrixXd Q(stateSize, stateSize);Q.setIdentity() * 0.001;Eigen::VectorXd x(stateSize);Eigen::VectorXd u(0);Eigen::VectorXd z_meas(measSize);z_meas.setZero();Eigen::VectorXd res(stateSize);         // 存储预测结果for (int i = 0; i < N; i++){if (i == 0)         // 初始值{x << data_x[i], data_y[i], 0, 0, 0, 0;kf.init(x, P, R, Q);continue;}kf.predict(A);                         // 预测z_meas << data_x[i], data_y[i];        // 观测res << kf.update(H, z_meas);           // 更新fout << data_x[i] << " " << data_y[i] << " " << res[0] << " " << res[1] << " " << res[2] << " " << res[3] << " " << res[4] << " " << res[5] << endl;}fout.close();return 0;}