文章目录
二维目标跟踪
对于一个二维目标跟踪问题，其中目标的状态量包括位置（ x , y x, y x,y）、速度（ v x , v y v_x, v_y vx,vy）和加速度（ a x , a y a_x, a_y ax,ay），Kalman滤波器的方程可以如下定义：
状态向量
定义状态向量 x k \mathbf{x}_k xk为：
x k = [ x y v x v y a x a y ] k \mathbf{x}_k = \begin{bmatrix} x \\ y \\ v_x \\ v_y \\ a_x \\ a_y \end{bmatrix}_k xk= xyvxvyaxay k
这里， x x x和 y y y表示目标在二维空间中的位置， v x v_x vx和 v y v_y vy表示目标在 x x x和 y y y方向上的速度， a x a_x ax和 a y a_y ay表示目标在 x x x和 y y y方向的加速度。
状态转移矩阵
状态转移矩阵 F \mathbf{F} F描述了状态向量如何随时间演变：
F = [ 1 0 Δ t 0 0.5 Δ t 2 0 0 1 0 Δ t 0 0.5 Δ t 2 0 0 1 0 Δ t 0 0 0 0 1 0 Δ t 0 0 0 0 1 0 0 0 0 0 0 1 ] \mathbf{F} = \begin{bmatrix} 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 \\ 0 & 0 & 0 & 1 & 0 & \Delta t \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} F= 100000010000Δt010000Δt01000.5Δt20Δt01000.5Δt20Δt01
其中， Δ t \Delta t Δt是时间步长。
观测矩阵
观测矩阵 H \mathbf{H} H将状态空间映射到观测空间。如果观测只包含位置信息，则：
H = [ 1 0 0 0 0 0 0 1 0 0 0 0 ] \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \end{bmatrix} H=[100100000000]
观测向量
观测向量 z k \mathbf{z}_k zk包含目标的观测位置：
z k = [ z x z y ] k \mathbf{z}_k = \begin{bmatrix} z_x \\ z_y \end{bmatrix}_k zk=[zxzy]k
其中， z x z_x zx和 z y z_y zy是从图像或其他传感器获得的目标位置观测值。
过程噪声协方差矩阵
过程噪声协方差矩阵 Q \mathbf{Q} Q描述了模型的不确定性：
Q = [ q x 0 0 0 0 0 0 q y 0 0 0 0 0 0 q v x 0 0 0 0 0 0 q v y 0 0 0 0 0 0 q a x 0 0 0 0 0 0 q a y ] \mathbf{Q} = \begin{bmatrix} q_x & 0 & 0 & 0 & 0 & 0 \\ 0 & q_y & 0 & 0 & 0 & 0 \\ 0 & 0 & q_{v_x} & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{v_y} & 0 & 0 \\ 0 & 0 & 0 & 0 & q_{a_x} & 0 \\ 0 & 0 & 0 & 0 & 0 & q_{a_y} \end{bmatrix} Q= qx000000qy000000qvx000000qvy000000qax000000qay
这里， q x , q y , q v x , q v y , q a x , q a y q_x, q_y, q_{v_x}, q_{v_y}, q_{a_x}, q_{a_y} qx,qy,qvx,qvy,qax,qay是对应状态的噪声方差。
观测噪声协方差矩阵
观测噪声协方差矩阵 R \mathbf{R} R描述了观测的不确定性：
R = [ r x 0 0 r y ] \mathbf{R} = \begin{bmatrix} r_x & 0 \\ 0 & r_y \end{bmatrix} R=[rx00ry]
这里， r x r_x rx和 r y r_y ry是观测噪声的方差。
卡尔曼增益
卡尔曼增益 K \mathbf{K} K用于更新状态估计：
K = P k ∣ k − 1 H T ( H P k ∣ k − 1 H T + R ) − 1 \mathbf{K} = \mathbf{P}{k|k-1} \mathbf{H}^T {(\mathbf{H} \mathbf{P}{k|k-1} \mathbf{H}^T + \mathbf{R} )}^{-1} K=Pk∣k−1HT(HPk∣k−1HT+R)−1
更新方程
x ^ k ∣ k = x ^ k ∣ k − 1 + K ( z k − H x ^ k ∣ k − 1 ) \mathbf{\hat{x}}{k|k} = \mathbf{\hat{x}}{k|k-1} + \mathbf{K} ( \mathbf{z}k - \mathbf{H} \mathbf{\hat{x}}{k|k-1} ) x^k∣k=x^k∣k−1+K(zk−Hx^k∣k−1)
P k ∣ k = ( I − K H ) P k ∣ k − 1 \mathbf{P}{k|k} = ( \mathbf{I} - \mathbf{K} \mathbf{H} ) \mathbf{P}{k|k-1} Pk∣k=(I−KH)Pk∣k−1
cpp
#include "eigen3/Eigen/Dense"
class KalmanFilter {
private:
Eigen::MatrixXd X; // 状态向量 [x, y, vx, vy, ax, ay]
Eigen::MatrixXd P; // 误差协方差矩阵
Eigen::MatrixXd F; // 状态转移矩阵
Eigen::MatrixXd H; // 观测矩阵
Eigen::MatrixXd Q; // 过程噪声协方差矩阵
Eigen::MatrixXd R; // 观测噪声协方差矩阵
Eigen::MatrixXd K; // 卡尔曼增益
public:
KalmanFilter() {
// 初始化状态向量
X = Eigen::MatrixXd::Zero(6, 1);
// 初始化误差协方差矩阵
P = Eigen::MatrixXd::Identity(6, 6) * 500;
// 定义状态转移矩阵
F = Eigen::MatrixXd::Zero(6, 6);
// F << 1, 0, 1, 0, 0.5, 0,
// 0, 1, 0, 1, 0, 0.5,
// 0, 0, 1, 0, 1, 0,
// 0, 0, 0, 1, 0, 1,
// 0, 0, 0, 0, 1, 0,
// 0, 0, 0, 0, 0, 1;
float dt = 1.0 / 24.0f; // 24fps
F << 1, 0, 1*dt, 0, 0.5*dt*dt, 0,
0, 1, 0, 1*dt, 0, 0.5*dt*dt,
0, 0, 1, 0, 1*dt, 0,
0, 0, 0, 1, 0, 1*dt,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1;
// 定义观测矩阵 (只观测位置)
H = Eigen::MatrixXd::Zero(2, 6);
H << 1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0;
// 定义过程噪声协方差矩阵
Q = Eigen::MatrixXd::Identity(6, 6) * 3.0;
// 定义观测噪声协方差矩阵
R = Eigen::MatrixXd::Identity(2, 2) * 0.5;
// 卡尔曼增益
K = Eigen::MatrixXd::Zero(6, 2);
}
void predict() {
X = F * X;
P = F * P * F.transpose() + Q;
}
void update(const Eigen::MatrixXd& Z) {
Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
X = X + K * (Z - H * X);
P = (Eigen::MatrixXd::Identity(6, 6) - K * H) * P;
}
void setState(const Eigen::MatrixXd& state) {
X = state;
}
Eigen::MatrixXd getState() const {
return X;
}
};