【滤波跟踪】基于EKF、时差和频差定位实现目标跟踪附matlab代码

1 内容介绍

三星时差无源定位系统多采用基于高程约束下时差测量值转换为距离差进行

定位，若卫星相对于辐射源存在运动，则截获到同一辐射源信号频率存在多普勒

频移。由于卫星间一直都处在运动状态，各观测站接收到的的信号将会产生不一

样的多普勒频移，从而通过多星间信号相关，就有可能会获取频差信息。本章研

究重点是在增加频差信息后，将会对三星时差定位系统的定位精度产生什么影响？并针对不同时差、频差测量误差对定位精度的影响作了对比分析。​

1.1 三星时差定位原理

时差定位是三星无源定位系统的主要定位方式之一。辐射源信号到达不同卫 星观测站的距离存在差异，导致信号到达各观测站的到达时间不同。由于无源定 位系统以被动接收辐射源信号的方式进行工作，通常难以直接观测辐射源到卫星 的距离（或到达时间观测量）。因此，在三星无源定位系统中，常以辐射源信号 到达不同卫星观测站的时间差作为主要观测量，通过观测各站间到达时间差即可 对辐射源进行定位[50]。 在三维空间中，同一辐射源信号到达 2 个空间完全隔离的卫星观测站间的到 达时间差，可确定一个以两观测站为焦点的半边双叶旋转双曲面。由 3 个卫星确 定的 2 个回转双曲面相交得到一条时差定位曲线，该曲线上的任意一点皆为辐射 源位置可行解。由于待定位辐射源通常位于地球表面一定高程，由时差定位曲线 与该高程约束面相交，即可唯一确定辐射源位置。然而，三星时差定位通常存在 模糊问题，即时差定位曲线与高程约束面通常存在 2 个交点。

在给定某些位置先 验信息条件下，就可唯一确定辐射源位置，这就是三星时差定位原理。在实际应用过程中，对于通信信号到达时间 TOA 观测量较难准确获得。对于 模拟信号，通常将到达各卫星观测站的辐射信号作互模糊函数，从而提取 TDOA 观测量；对于雷达信号，就可以直接测量脉冲上升沿时间作为信号到达时间。

1.2 三星频差定位原理

由于观测站与目标辐射源之间发生了相对运动，从而引起观测站接收到的信

号频率与辐射源发出频率出现一个差值，该差值叫作多普勒频移。而对于 2 个空 

间隔离的卫星观测站，接收到同一个辐射源信号的多普勒频移也是不同的，观测

站间同样也存在一个多普勒频差。根据卫星观测站间存在的频差可确定一个等频

差曲面，利用三个观测站可得到 2 个等频差曲面。2 个等频差曲面相交得到一个等

频差交线，辐射源就在该等频差曲线上。对于地球表面目标，利用地球模型与等

频差曲线相交，从而实现多普勒频率对辐射源定位。

2 仿真代码

<span style="color:#333333"><span style="background-color:rgba(0, 0, 0, 0.03)"><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span></code><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span>%时差定位分析   可以运行 </code><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span></code><code>clear all;</code><code>clc;</code><code>c=<span style="color:#0e9ce5">3</span>e5;%光速</code><code>w=<span style="color:#0e9ce5">10</span>;%误差ns</code><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span>飞机距离</code><code>x=-<span style="color:#0e9ce5">200</span><span style="color:#dd1144">:</span><span style="color:#0e9ce5">2</span><span style="color:#dd1144">:</span><span style="color:#0e9ce5">200</span>;</code><code>y=-<span style="color:#0e9ce5">200</span><span style="color:#dd1144">:</span><span style="color:#0e9ce5">2</span><span style="color:#dd1144">:</span><span style="color:#0e9ce5">200</span>;</code><code>z=<span style="color:#0e9ce5">5</span>;</code><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span>方型</code><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span>载机布站<span style="color:#0e9ce5">1.1</span>（机间距：<span style="color:#0e9ce5">15</span>km）</code><code>% x<span style="color:#0e9ce5">0</span>=[<span style="color:#0e9ce5">0</span>,<span style="color:#0e9ce5">15</span>,<span style="color:#0e9ce5">0</span>,<span style="color:#0e9ce5">15</span>]<span style="color:#dd1144">';</span></code><code>% y0=[0,15,15,0]';</code><code>% z<span style="color:#0e9ce5">0</span>=[<span style="color:#0e9ce5">1</span> ,<span style="color:#0e9ce5">0</span>.<span style="color:#0e9ce5">9</span>,<span style="color:#0e9ce5">0</span>.<span style="color:#0e9ce5">9</span> ,<span style="color:#0e9ce5">0</span>.<span style="color:#0e9ce5">9</span>]<span style="color:#dd1144">';</span></code><code>%%%%%%%%%%%%%%%%%%%%%%%%%1.2 （30km,载机布站方形）</code><code>% x0=[0,30,0,30]';</code><code>% y<span style="color:#0e9ce5">0</span>=[<span style="color:#0e9ce5">0</span>,<span style="color:#0e9ce5">30</span>,<span style="color:#0e9ce5">30</span>,<span style="color:#0e9ce5">0</span>]<span style="color:#dd1144">';</span></code><code>% z0=[1 ,0.9,0.9 ,0.9]';</code><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span>Y型</code><code><span style="color:#dd1144">%%%</span><span style="color:#dd1144">%%%</span>载机布站<span style="color:#0e9ce5">2.1</span>（<span style="color:#0e9ce5">15</span>km、主在Y中间）</code><code>% x<span style="color:#0e9ce5">0</span>=[<span style="color:#0e9ce5">0</span>,<span style="color:#0e9ce5">13</span>,-<span style="color:#0e9ce5">13</span>,<span style="color:#0e9ce5">0</span>]<span style="color:#dd1144">';</span></code><code>% y0=[0,7.5,7.5,-15]';</code><code>% z<span style="color:#0e9ce5">0</span>=[<span style="color:#0e9ce5">1</span> ,<span style="color:#0e9ce5">0</span>.<span style="color:#0e9ce5">9</span>,<span style="color:#0e9ce5">0</span>.<span style="color:#0e9ce5">9</span> ,<span style="color:#0e9ce5">0</span>.<span style="color:#0e9ce5">9</span>]<span style="color:#dd1144">';</span></code><code>%%%%%%载机布站2.2（30km、主在Y中间）</code><code>x0=[0,26,-26,0]';</code><code>y<span style="color:#0e9ce5">0</span>=[<span style="color:#0e9ce5">0</span>,<span style="color:#0e9ce5">15</span>,<span style="color:#0e9ce5">15</span>,-<span style="color:#0e9ce5">30</span>]<span style="color:#dd1144">';</span></code><code>z0=[1 ,0.9,0.9 ,0.9]';</code><code>dt=zeros(<span style="color:#0e9ce5">1</span>,<span style="color:#0e9ce5">3</span>);</code><code>dR=zeros(<span style="color:#0e9ce5">1</span>,<span style="color:#0e9ce5">3</span>);</code><code><span style="color:#ca7d37">for</span> n=<span style="color:#0e9ce5">1</span><span style="color:#dd1144">:</span><span style="color:#0e9ce5">3</span></code><code>    dR(n)=w*<span style="color:#0e9ce5">1</span>e-<span style="color:#0e9ce5">9</span>*c;      </code><code><span style="color:#ca7d37">end</span></code><code>dr2=dR.^<span style="color:#0e9ce5">2</span>;</code><code>Pn=diag([dr2(<span style="color:#0e9ce5">1</span>) dr2(<span style="color:#0e9ce5">2</span>) dr2(<span style="color:#0e9ce5">3</span>)]);</code><code><span style="color:#ca7d37">for</span> i=<span style="color:#0e9ce5">1</span><span style="color:#dd1144">:length</span>(x)</code><code>    <span style="color:#ca7d37">for</span> j=<span style="color:#0e9ce5">1</span><span style="color:#dd1144">:length</span>(y)</code><code>        <span style="color:#ca7d37">for</span> k=<span style="color:#0e9ce5">1</span><span style="color:#dd1144">:length</span>(z)</code><code>%         k=<span style="color:#0e9ce5">1</span>;</code><code>            r1=sqrt((x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">1</span>))^<span style="color:#0e9ce5">2</span>+(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">1</span>))^<span style="color:#0e9ce5">2</span>+(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">1</span>))^<span style="color:#0e9ce5">2</span>);</code><code>            r2=sqrt((x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">2</span>))^<span style="color:#0e9ce5">2</span>+(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">2</span>))^<span style="color:#0e9ce5">2</span>+(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">2</span>))^<span style="color:#0e9ce5">2</span>);</code><code>            r3=sqrt((x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">3</span>))^<span style="color:#0e9ce5">2</span>+(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">3</span>))^<span style="color:#0e9ce5">2</span>+(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">3</span>))^<span style="color:#0e9ce5">2</span>);</code><code>            r4=sqrt((x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">4</span>))^<span style="color:#0e9ce5">2</span>+(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">4</span>))^<span style="color:#0e9ce5">2</span>+(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">4</span>))^<span style="color:#0e9ce5">2</span>);</code><code>            R=[r1,r2,r3,r4];</code><code>​</code><code>            cx=[(x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">1</span>))/r1,(x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">2</span>))/r2,(x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">3</span>))/r3,(x(i)-x<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">4</span>))/r4];</code><code>            cy=[(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">1</span>))/r1,(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">2</span>))/r2,(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">3</span>))/r3,(y(j)-y<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">4</span>))/r4];</code><code>            cz=[(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">1</span>))/r1,(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">2</span>))/r2,(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">3</span>))/r3,(z(k)-z<span style="color:#0e9ce5">0</span>(<span style="color:#0e9ce5">4</span>))/r4];</code><code>            C=[cx(<span style="color:#0e9ce5">2</span>)-cx(<span style="color:#0e9ce5">1</span>),cy(<span style="color:#0e9ce5">2</span>)-cy(<span style="color:#0e9ce5">1</span>),cz(<span style="color:#0e9ce5">2</span>)-cz(<span style="color:#0e9ce5">1</span>);</code><code>               cx(<span style="color:#0e9ce5">3</span>)-cx(<span style="color:#0e9ce5">1</span>),cy(<span style="color:#0e9ce5">3</span>)-cy(<span style="color:#0e9ce5">1</span>),cz(<span style="color:#0e9ce5">3</span>)-cz(<span style="color:#0e9ce5">1</span>);</code><code>               cx(<span style="color:#0e9ce5">4</span>)-cx(<span style="color:#0e9ce5">1</span>),cy(<span style="color:#0e9ce5">4</span>)-cy(<span style="color:#0e9ce5">1</span>),cz(<span style="color:#0e9ce5">4</span>)-cz(<span style="color:#0e9ce5">1</span>)];</code><code>            B=inv(C.<span style="color:#dd1144">'*C)*C.'</span>;</code><code>            Pd=B*Pn*B.<span style="color:#dd1144">';</span></code><code>            Gxy(i,j)=abs(sqrt(Pd(1,1)+Pd(2,2)));</code><code>            Gz(i,j)=abs(Pd(3,3));</code><code>        end</code><code>    end</code><code>end</code><code>figure(1);  %GDOP</code><code>M=0.02:0.2:1.25;</code><code>fig=contour(x,y,Gxy,M);clabel(fig);</code><code>xlabel( 'x/km<span style="color:#dd1144">');</span></code><code>ylabel('y/km<span style="color:#dd1144">');</span></code><code>hold on;</code><code>grid on;</code><code>title('<span style="color:#0e9ce5">4</span>站时差定位GDOP(x,y)图仿真<span style="color:#dd1144">');</span></code><code>figure(2);  %GDOP</code><code>M=0.1:0.2:2.5;</code><code>fig1=contour(x,y,Gz,M);clabel(fig1);</code><code>xlabel( 'x/km<span style="color:#dd1144">');</span></code><code>ylabel('y/km<span style="color:#dd1144">');</span></code><code>hold on;</code><code>grid on;</code><code>title('<span style="color:#0e9ce5">4</span>站时差定位GDOP(z)图仿真<span style="color:#dd1144">');</span></code><code>​</code></span></span>

3 运行结果

4 参考文献

[1]朱国辉. 基于时差频差的多站无源定位与跟踪算法研究. Diss. 西安电子科技大学.

[2]张艳. 三星时差频差联合目标定位与跟踪算法研究[D]. 西安电子科技大学.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。