面向假设的多假设目标跟踪算法-Python3.X实现
-
- 前言
- 1. tracker.py
-
- 1.1 _normalize_log_sum(items)
- 1.2 LocalHypothesis类
- 1.3 Track类
-
- 1.3.1 id
- 1.3.2 __call__(lhyp_id)
- 1.3.3 add(local_hypothesis)
- 1.3.4 estimate(lhyp_id=None)
- 1.3.5 is_within(volume)
- 1.3.6 log_likelihood_ratio(lhyp_id=None)
- 1.3.7 log_likelihood_ratio_max(lhyp_id=None)
- 1.3.8 log_likelihood(lhyp_id)
- 1.3.9 dead_local_hyps
- 1.3.10 confirmed_local_hyps
- 1.3.12 select(lhyp_ids)
- 1.3.13 predict(t_now)
- 1.3.14 update(Z, volume, t_now)
- 1.4 Tracker类
-
- 1.4.1 create_track_trees(detections, volume, targetmodel, t_now)
- 1.4.3 update_global_hypotheses(track_updates,Z,targetmodel,volume,M, weight_threshold,t_now)
- 1.4.4 prune_dead
- 1.4.5 hypothesis_prune静态方法(可通过类名/实例名调用)
- 1.4.6 hypothesis_cap静态方法
- 1.4.7 estimates
- 1.4.8 predict
- 1.4.9 terminate_tracks
- 1.4.10 update(detections, volume, targetmodel, t_now)
- 1.4.11 process(detections, volume, targetmodel, t_now)
- 2. data_association.py
- 3. gaussian.py
- 4. generation.py
-
- 4.1 _ground_truth_fixed_step(t_length,x_birth,t_birth,t_death, motionmodel,dt)函数
- 4.2 ground_truth(t_end, x_birth, t_birth, t_death, motionmodel, dt=1.0)
- 4.3 measurements(ground_truth, measmodel, P_D, lambda_c, range_c)
- 4.4 random_ground_truth(t_end, init_state_density, init_lambda, P_survival, motionmodel, dt)
- 5. motionmodel.py
- 6. measmodel.py
- 7.target.py
- 8.cv_target.py
- 9. scan_volume.py
- 10. murtyPy.py
- 11. single_run.py
- 12.constant
前言
这是我在交大读研期间做滤波与目标跟踪算法方向时的可公开代码。这是关于雷达多目标跟踪的仿真程序。这部分可公开代码所给出多个点目标仿真数据的生成方式是合理的,采用仿真场景为NCV(近似匀速直线模型)、NCA(近似匀加速直线模型),使用HO-MHT(面向假设的多假设目标跟踪)算法(采用murty算法优化)进行数据关联。对于基础原理请参看Blackman的相应文章以及其他的相应论文。本文和代码不牵扯真实数据,仅供学习使用。代码由Python3.X编写,在代码中我给出较为详细的注释。
语言:Python3.x
外部依赖包:numpy, scipy, tqdm.
注:使用murty算法进行优化,其效果和TO-MHT类似
代码地址:https://github.com/Tsingpeng/HO-MHT
1. tracker.py
包含3个类(局部假设管理,航迹(假设簇)管理,全局假设管理)以及内部对数归一化函数.
1.1 _normalize_log_sum(items)
对给定的数组items
进行对数归一化处理.
我们先得到items
从大到小的索引序列,从而找到items
中的最大值 w 0 w_0 w0,从而得到对数和为:
l s = w 0 + l n [ 1 + ∑ i = 1 n e x p ( w i − w 0 ) ] ls = w_0+ln[1+\sum_{i=1}^{n}exp(w_i-w_0)] ls=w0+ln[1+i=1∑nexp(wi−w0)]
N o r m = i t e m s − l s Norm = items - ls Norm=items−ls
其中 { w 0 , w 1 , ⋯ , w n } \{w_0,w_1,\cdots,w_n\} {
w0,w1,⋯,wn}为items
的降序排列的元素,返回归一化值Norm和对数和ls.
1.2 LocalHypothesis类
用以产生局部假设,每创建一次实例就会分配一个id
,创建时的初始化变量包括:target,LLR,LLR_max,loglikelihood.
1.2.1 id()
返回局部假设的id
1.2.2target()
深拷贝变量target
1.2.3 density()
深拷贝变量target.density()
1.2.4 predict(t_now)
返回当前的预测结果
1.2.5 log_likelihood
返回对数似然
1.2.6 is_dead
返回目标是否消失
1.2.7 is_confirmed
返回目标是否确认
1.2.8 new_from_hit(cls, self, z, hit_llhood, t_hit)类方法!!!
有量测时从新的潜在航迹得到新的似然比 L ( k ) = L ( k − 1 ) + Δ L ( k ) L(k)=L(k-1)+\Delta L(k) L(k)=L(k−1)+ΔL(k),返回新的局部假设
1.2.9 new_from_miss(cls, self, miss_llhood, t_now)类方法!!!
没有量测时得到新的似然比 L ( k ) = L ( k − 1 ) + Δ L ( k ) L(k)=L(k-1)+\Delta L(k) L(k)=L(k−1)+ΔL(k),返回新的局部假设
1.3 Track类
航迹:假设的集合(位于椭圆门限内)
初始化变量有:lhyps→{id:object},trid→每创建一个实例,分配一个track_id.
1.3.1 id
返回当前Track的id
1.3.2 call(lhyp_id)
像函数一样调用当前对象.如:A=Track(…);Track(lhyp_id)
判断输入的hypo_id在lhyps中否,若在返回id对应的object,否则返回None
1.3.3 add(local_hypothesis)
添加假设local_hypothesis到lhyps中
1.3.4 estimate(lhyp_id=None)
返回density:
- 不指定假设的id,返回所有假设的density
- 指定假设的id的话,返回这个id对应的假设的density
1.3.5 is_within(volume)
返回量测值的预测值是否在量测体积内
1.3.6 log_likelihood_ratio(lhyp_id=None)
返回对数似然比:
- 未指定,返回所有的LLR
- 指定ID,返回指定ID的LLR
1.3.7 log_likelihood_ratio_max(lhyp_id=None)
返回最大似然比:
- 未指定,返回所有的LLR
- 指定ID,返回指定ID的LLR
1.3.8 log_likelihood(lhyp_id)
返回指定ID的似然
1.3.9 dead_local_hyps
返回已消失的局部假设id列表
1.3.10 confirmed_local_hyps
返回已确认的局部假设id列表
#### 1.3.11 terminate(lhyp_ids)
更新假设:保留不在输入lhyp_idx中的假设,即lhyp_ids是终止假设的集合(列表)
1.3.12 select(lhyp_ids)
更新假设:保留在输入lhyp_idx中的假设,即lhyp_ids是选择假设的集合(列表)
1.3.13 predict(t_now)
预测:对每个假设进行预测
1.3.14 update(Z, volume, t_now)
更新:更新局部假设(对于每个局部假设进行处理)
- 得到在椭圆门内的量测和量测索引
- 得到Density.predicted_likelihood
- 得到有量测存在下的 Δ L ( k ) = l n f ( z , z k k 1 , S ) + l n ( P D / P F ) \Delta L(k) = ln f(z,zkk_1,S)+ln(P_D/P_F) ΔL(k)=lnf(z,zkk1,