汽车学堂 自 动 驾 驶 决 策 与 控 制 算 法——卡尔曼滤波应用

---

一、应用KF的流程

1. 建立系统的状态空间模型
2. 离散化
3. 给定粗值，然后递归
   

二、运动模型

1.匀速模型（CV）

2.匀加速模型（CA）

3.singer模型

4.匀速转弯模型（CT）

三、数据关联

1.时间维度上的关联

2.空间维度上的关联

3.欧式距离法

4.匈牙利算法

指派问题是一种特殊的整数规划问题，匈牙利算法就是解决指派问题最好的方法。

a)获得初始解：圈零/划零操作

① 将时间矩阵C的每一行都减去相应行的最小元素和每一列都减去相应列的最小元素，使每
一行和每一列都含有零；

② 从最小零数的行或列开始，将“零”圈起来，并划去它所在行和所在列的其它零；
③ 反复做②，直到所有零被圈起或被划掉为止。得到初始解；
④ 判断是否为最优解：圈起的零的个数是否等于n。

b)确定调整行和列

① 在没有圈起来的零所在行上打“√”；
② 在打“√”行中所有零所在的列打“√”；
③ 在打“√”列中含有圈起零的行上打“√”；
④ 反复执行②和③两步，直到不能打“√”为止；
⑤ 用直线划去打“√”的列和不打“√”的行，没有划去的行构成调整的行，划去的列构成调整列。

c)调整可行解的方法

① 在调整行中寻找最小的元素，将它作为调整量；
② 将调整行各元素减去调整量，对调整列中各元素加上调整量。
③ 再次执行“圈零”和“划零”的操作，并循环以上的步骤，指导圈起的零数等于n为止。

四、多目标跟踪

① 建立目标运动模型（CV/CA/CT/singer）
② 离散化
③ 给定初值，然后预测
④ 预测值与实测值在时间维度上关联
⑤ 如果关联失败，说明出现了误报/新目标、漏报/目标丢失；
• 针对误报/新目标，初始化一个新的KF；
• 针对漏报/目标丢失，利用模型预测。
⑥ 利用关联上的测量值反馈校正预测值