赞
踩
卡尔曼滤波是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。
通俗来说就是,线性数学模型算出预测值+传感器测量值=更准确的测量值。根据数学模型,由第 k k k 时刻的值递推得到第 k + 1 k+1 k+1 时刻的预测值,结合第 k + 1 k+1 k+1 时刻的观测值,得到第 k + 1 k+1 k+1 时刻更精准的值。
卡尔曼滤波主要用于 线性高斯系统。
(1)线性高斯系统表达
状态方程:
x k = A x k − 1 + B u k + w k \boldsymbol{x}_k = \boldsymbol{A}\boldsymbol{x}_{k-1}+\boldsymbol{B}\boldsymbol{u}_k+\boldsymbol{w}_k xk=Axk−1+Buk+wk
观测方程:
z k = H x k + v k \boldsymbol{z}_k = \boldsymbol{H}\boldsymbol{x}_k+\boldsymbol{v}_k zk=Hxk+vk
其中, x k \boldsymbol{x}_k xk 为状态量, z k \boldsymbol{z}_k zk 为观测量, A \boldsymbol{A} A 为状态转移矩阵, B k \boldsymbol{B}_k Bk 为控制输入矩阵, H \boldsymbol{H} H 为状态观测矩阵。
w k \boldsymbol{w}_k wk 是过程噪声,服从高斯分布, w k \boldsymbol{w}_k wk 是观测噪声,也服从高斯分布,即:
w k ∼ N ( 0 , Q ) \boldsymbol{w}_k \sim N(0, \boldsymbol{Q}) wk∼N(0,Q)
v k ∼ N ( 0 , R ) \boldsymbol{v}_k \sim N(0, \boldsymbol{R}) vk∼N(0,R)
其中 Q \boldsymbol{Q} Q 是过程噪声的协方差, R \boldsymbol{R} R 是观测噪声的协方差。
卡尔曼滤波包括预测和更新两步。
(2)预测(先验)
预测是根据上一时刻的状态量,由状态方程预测出下一时刻的状态量 x ^ k − \hat{\boldsymbol{x}}_k^{-} x^k− ,以及状态量误差协方差的先验估计矩阵 P k − \boldsymbol{P}_k^{-} Pk−。这是没有加观测值的。
x ^ k − = A x ^ k − 1 + B u k \hat{\boldsymbol{x}}_k^{-} = \boldsymbol{A}\hat{\boldsymbol{x}}_{k-1}+\boldsymbol{B}\boldsymbol{u}_k x^k−=Ax^k−1+Buk
P k − = A P k − 1 A T + Q \boldsymbol{P}_k^{-}=\boldsymbol{AP}_{k-1}\boldsymbol{A}^T+\boldsymbol{Q} Pk−=APk−1AT+Q
其中, A x ^ k − 1 \boldsymbol{A}\hat{\boldsymbol{x}}_{k-1} Ax^k−1 是上一时刻的最优估计。
(3)更新(后验)
加入观测,对预测值进行更新校正,得到最优后验估计。
首先计算增益矩阵
K k = P k − H T ( H P k − H T + R ) − 1 \boldsymbol{K}_k=\boldsymbol{P}_k^{-}\boldsymbol{H}^T(\boldsymbol{H}\boldsymbol{P}_k^{-}\boldsymbol{H}^T+\boldsymbol{R})^{-1} Kk=Pk−HT(HPk−HT+R)−1
更新状态量及其协方差矩阵
x ^ k = x ^ k − + K k ( z k − H x ^ k − ) \hat{\boldsymbol{x}}_k = \hat{\boldsymbol{x}}_k^{-} + \boldsymbol{K}_k(\boldsymbol{z}_k-\boldsymbol{H}\hat{\boldsymbol{x}}_k^{-}) x^k=x^k−+Kk(zk−Hx^k−)
P k = ( I − K k H ) P k − \boldsymbol{P}_k=(\boldsymbol{I}-\boldsymbol{K}_k\boldsymbol{H})\boldsymbol{P}_k^{-} Pk=(I−KkH)Pk−
以雷达追踪目标为背景,系统的状态方程为
[
x
y
V
x
V
y
a
x
a
y
]
k
+
1
=
[
1
0
δ
t
0
0.5
δ
t
2
0
0
1
0
δ
t
0
0.5
δ
t
2
0
0
1
0
δ
t
0
1
0
0
1
0
δ
t
0
0
0
0
1
0
0
0
0
1
0
1
]
[
x
y
V
x
V
y
a
x
a
y
]
k
观测方程
[
x
y
]
k
+
1
=
[
1
0
0
0
0
0
0
1
0
0
0
0
]
[
x
y
V
x
V
y
a
x
a
y
]
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_k MatrixXd 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.01 double 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; }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。