三维匀速运动CV模型
Write By Champrin On 2022-10-22
GUET Evolution Team Visual Group
目录
这是2022赛季视觉组代码中卡尔曼滤波器（KF线性滤波器）所使用的模型，推导结果也与代码中的一致
状态变量X
对于三维坐标\((x,y,z)\) ，作为装甲板进行坐标结算后装甲板中心点的位置\((x,y,z)\)
\[X = \begin{bmatrix} x_1 \\ y_1 \\ z_1 \\ x_2 \\ y_2 \\ z_2 \end{bmatrix} 其中 \begin{aligned} x_1 & = x \\ y_1 & = y \\ z_1 & = z \\ x_2 & = \dot{x} \\ y_2 & = \dot{y} \\ z_2 & = \dot{z} \end{aligned}\]状态方程
\[\begin{aligned} x_{1,k} & = x_{1,k-1} + \Delta{T}x_{2,k-1} + w_{1,k-1} \\ x_{2,k} & = x_{2,k-1} + w_{2,k-1} \\ y_{1,k} & = y_{1,k-1} + \Delta{T}y_{2,k-1} + w_{3,k-1} \\ y_{2,k} & = y_{2,k-1} + w_{4,k-1} \\ z_{1,k} & = z_{1,k-1} + \Delta{T}z_{2,k-1} + w_{5,k-1} \\ z_{2,k} & = z_{2,k-1} + w_{6,k-1} \\ \end{aligned}\]转换为矩阵形式
\[X_k = \begin{bmatrix} 1 & \Delta{T} & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & \Delta{T} & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & \Delta{T} \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} x_{1,k-1} \\ y_{1,k-1} \\ z_{1,k-1} \\ x_{2,k-1} \\ y_{2,k-1} \\ z_{2,k-1} \end{bmatrix} + W_{k-1} \]即
\[X_k = AX_{k-1} + W_{k-1} \]其中
状态转移矩阵A
过程噪声矩阵W
\[W_{k-1} = \begin{bmatrix} w_{1,k-1} \\ w_{2,k-1} \\ w_{3,k-1} \\ w_{4,k-1} \\ w_{5,k-1} \\ w_{6,k-1} \end{bmatrix} \]过程噪声协方差矩阵Q
\[Q = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \]测量状态变量Z
对于测量方程，速度无法测量，只能通过识别装甲板进行坐标结算获得装甲板中心点的位置，即位置\((x_1,y_1,z_1)\)
\[Z = \begin{bmatrix} z_1 \\ z_2 \\ z_3 \end{bmatrix} 其中 \begin{aligned} z_1 & = x_1 \\ z_2 & = y_1 \\ z_3 & = z_1 \\ \end{aligned} \]测量方程
\[\begin{aligned} z_{1,k} & = x_{1,k} + v_{1,k} \\ z_{2,k} & = y_{1,k} + v_{2,k} \\ z_{3,k} & = z_{1,k} + v_{3,k} \end{aligned} \]转换为矩阵形式
\[Z_k = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix}\begin{bmatrix} x_{1,k} \\ y_{1,k} \\ z_{1,k} \\ x_{2,k} \\ y_{2,k} \\ z_{2,k} \end{bmatrix} + V_k \]即
\[Z_k = HX_k + V_k \]其中
状态观测矩阵H
测量噪声矩阵V
\[V_k = \begin{bmatrix} v_{1,k} \\ v_{2,k} \\ v_{3,k} \end{bmatrix} \]测量噪声协方差矩阵R
\[R = \begin{bmatrix} 10 & 0 & 0\\ 0 & 10 & 0\\ 0 & 0 & 10\\ \end{bmatrix} \]\(P_0\) 及 \(\hat{X}_0\)
状态估计误差协方差矩阵 \(P_0\) 为 \(n*n\) 的单位矩阵
状态估计值的初值 \(\hat{X}_0 = \begin{bmatrix} x_0 \\ y_0 \\ z_0 \\ 0 \\ 0 \\ 0 \end{bmatrix}\) ， \((x_0,y_0,z_0)\) 为第一次识别出的装甲板矩形的中心点
以上相关变量和矩阵都与上赛季2022的视觉代码中的线性卡尔曼滤波器变量和矩阵一致
个人认为对于R：
第\(k\)时刻，通过识别装甲板并坐标结算，得到装甲板的位置，是较为准确的，这是测量结果；因此\(R\)的对角线上的协方差应该设置得较为小，表示相信测量结果
但是坐标结算存在着波动，即使瞄一个静止不动的哨兵，当R设置的较为小时，同时坐标结算存在波动，这样会导致云台瞄准左右摇摆，因为\(R\)小会使得状态估计值更加相信测量结果，所以云台就同结算的波动而左右摇摆，因此\(R\)不得不设置为一个较大的值，但\(R\)设置得较大，会影响状态估计值，即预测的结果
若相信测量结果，预判理应会更加准确；但要使\(R\)设置较为小，要先解决好坐标结算波动的问题