激光检测 Laser Measurement

2022-07-06 14:46:41 浏览数 (2)

状态预测 State Prediction

线性模型假设,物体在运动时,每段时间间隔中速度恒定。实际上,每次测量时间之间的间隔是不定的,物体的加速也是不定的

时间和加速度的不确定性决定了过程噪声process Noise

下图引入了状态转移矩阵

过程协方差矩阵 Process Covariance Matrix

由于状态向量只包含位置和速度信息,实际上加速度在模型中是作为随机噪声的

由于加速度不确定,所以直接当成随机成分。然后对上面的式子求导,就得到下面的随机加速向量v

v是服从于N(0, Q)分布

再把v分解成两个矩阵。一个4x2的矩阵G,其中不包含随机变量。一个2x1的矩阵a,包含随机加速项。

根据定义,协方差矩阵Q又是v乘上v的转置的数学期望。由于G不包含随机项,所以移到了数学期望的外面

剩下的就是ax的方差,ay的方差,ax和ay的协方差。由于ax和ay不相关,所以协方差是0. 前前后后放到一起,就是下面这玩意儿

激光检测 Laser Measurement

使用激光传感器,获取点云数据,探测物体。利用卡尔曼滤波进行转台预测

预测效果如下

选取近似直线部分放大,发现预测跟实际物体运行高度吻合

选取转向部分放大,预测偏离实际运行轨迹。因为在每个小的时间间隔中,我们一直假设方向不变

0 人点赞