用C/C++实现输入IQ数据，计算时差（IQ数据互相关），完成TDOA定位（Chan算法）

文末贴源码链接

需求：已知接收机IQ数据，根据IQ数据做互相关，求信号时差，最终通过TDOA（Chan算法）定位发射机坐标。输入输出坐标均为经纬度坐标，而在TDOA计算中，需要笛卡尔坐标系坐标，涉及坐标转换问题。

头文件：一共包含4个算法

/***************************************************************
TDOA算法1：
****************************************************************/

//场强超过阈值记录时间戳
struct INPUT
{
	double jcdwz[2];//经纬度
	double time;//时间戳
	double* cq;//场强数组
	int cqLen;//场强数组的长度
	int deviceId;//设备ID，只能是1,2,3,4

	double threshold;//阈值
	double delay[4] = { 0,0,0,0 };//四个接收站同步延迟，无延迟则全部输入0，单位：纳秒
}input;

//场强超过阈值记录时间戳,直到记录了4台设备开始TDOA计算
//输出
//int flag;//0表示没有计算，1表示可以读取TDOA结果
//double max;//最大场强值
//double llOutput[2];//TDOA定位结果
void Fun_TDOA(INPUT input, int* flag, int* max, double* llOutput);


/***************************************************************
TDOA算法2：反向求时间差，为算法1提供仿真数据
****************************************************************/

//仿真输入结构体，针对Fun_TDOA的仿真
struct INPUT_Simulation
{
	double BS[2];//接收站经度、纬度，单位：度、度
	double S[2];//发射站的经度、纬度
}input_simulation;
//北京位于东经115.7°—117.4°,北纬39.4°—41.6°
//        经度   纬度      Fun_TDOA_Simulation计算结果 
//发射站：116.25 39.54     
//接收站1：116   39        274949
//接收站2：117   40        356044
//接收站3：116   40        240672
//接收站4：117   39        380055
double Fun_TDOA_Simulation(INPUT_Simulation input_simulation);


/***************************************************************
TDOA算法3：
****************************************************************/

//4路IQ同时输入
struct INPUT4IQ
{
	double jcdwz1[2];//经纬度
	double *I1;//I数据
	double *Q1;//Q数据
	double fs1;//IQ采样频率

	double jcdwz2[2];
	double *I2;
	double *Q2;
	double fs2;

	double jcdwz3[2];
	double *I3;
	double *Q3;
	double fs3;

	double jcdwz4[2];
	double *I4;
	double *Q4;
	double fs4;

	int len;//IQ数据长度
}input4IQ;

//4路IQ同时输入
//输出
//double llOutput[2];//TDOA定位结果
void Fun_TDOA_Input4IQ(INPUT4IQ input4IQ, double* llOutput);


/***************************************************************
TDOA算法4：
****************************************************************/

//每次输入1路IQ，在完成一次TDOA前保证输入的len都一样
struct INPUT1IQ
{
	double jcdwz[2];//经纬度
	double *I;//I数据
	double *Q;//Q数据
	double fs;//IQ采样频率

	int deviceId;//设备ID，只能是1,2,3,4
	int len;//IQ数据长度
}input1IQ;

//每次输入1路IQ,直到记录了4台不同设备的IQ数据开始TDOA计算
//输出
//int flag;//0表示没有计算，1表示可以读取TDOA结果
//double llOutput[2];//TDOA定位结果
void Fun_TDOA_Input1IQ(INPUT1IQ input1IQ, int* flag, double* llOutput);

算法1：给定时间戳，直接计算TDOA

执行流程：主函数反复调用算法1，每次输入一台设备的数据，数据中有一组场强值，如果该场强值中有超过指定阈值的值，则记录该设备的坐标和时间戳。直到记录了4台不同设备的数据，开始执行TDOA定位。最后输出定位结果，即定位的发射机的坐标（涉及经纬度与笛卡尔坐标系转换问题）。（为什么要输入场强值？项目需要，其实直接给4个时间戳就能做TDOA）

注：理论上3台设备的时间戳就能实现TDOA，此处用4台，因为一开始想做经纬高，三维空间定位，但高度的定位上存在问题，就去掉了这个功能，但保留4台设备，以提高经纬度定位的精度。TDOA定位中使用了Chan算法。

算法2：反向求时间戳，为算法1提供仿真数据

算法3、算法4：算法3与算法4的唯一区别在于一次输入4路IQ数据，还是一次输入1路IQ数据。与算法1的区别在于，获取时间戳（或时间差）的方式不同，但最终目的都是获取时间戳（或时间差）计算TDOA。

4路IQ数据两两做互相关，计算时间差：

function [time_delay] = calculate_delay(I1,Q1,I2,Q2,fs1,fs2,len)
%#codegen
    currFs=fs1;
    %重新采样
    %fs0 是重采样的频率
    fs0=10000000;
    %最长长度，防止因为采样频率增加导致内存不足
    targetLen=10000000;
    if fs0 > fs1 && fs0 > fs2
        IQ1L=ceil(targetLen*fs1/fs0);
        IQ2L=ceil(targetLen*fs2/fs0);
        if IQ1L < 500
            IQ1L=500;
        end
        if IQ2L < 500
            IQ2L=500;
        end
        
        if IQ1L > len
            IQ1L=len;
        end
        if IQ2L > len
            IQ2L=len;
        end
        %从fs1变为fs0
        I1_resample=resample(I1(1:IQ1L),fs0,fs1);
        Q1_resample=resample(Q1(1:IQ1L),fs0,fs1);
        %从fs2变为fs0
        I2_resample=resample(I2(1:IQ2L),fs0,fs2);
        Q2_resample=resample(Q2(1:IQ2L),fs0,fs2);
        currFs=fs0;
    %统一为大的采样频率
    elseif fs1 > fs2
        IQ2L=ceil(targetLen*fs2/fs1);
        if IQ2L < 500
            IQ2L=500;
        end
        if IQ2L > len
            IQ2L=len;
        end
        I2_resample=resample(I2(1:IQ2L),fs1,fs2);
        Q2_resample=resample(Q2(1:IQ2L),fs1,fs2);
        I1_resample=I1;
        Q1_resample=Q1;
        currFs=fs1;
    elseif fs2 > fs1
        IQ1L=ceil(targetLen*fs1/fs2);
        if IQ1L < 500
            IQ1L=500;
        end
        if IQ1L > len
            IQ1L=len;
        end
        I1_resample=resample(I1(1:IQ1L),fs2,fs1);
        Q1_resample=resample(Q1(1:IQ1L),fs2,fs1);
        I2_resample=I2;
        Q2_resample=Q2;
        currFs=fs2;
    else
        I1_resample=I1;
        Q1_resample=Q1;
        I2_resample=I2;
        Q2_resample=Q2;
    end
    len=min(length(I1_resample),length(I2_resample));
    I1_computer=I1_resample(1:len);
    Q1_computer=Q1_resample(1:len);
    I2_computer=I2_resample(1:len);
    Q2_computer=Q2_resample(1:len);
    
    %I互相关
    [delay1,zuobiao1]=xcorr(I1_computer,I2_computer);
    time_delay1=max(zuobiao1(delay1==max(delay1)));
    %Q互相关
    [delay2,zuobiao2]=xcorr(Q1_computer,Q2_computer);
    time_delay2=max(zuobiao2(delay2==max(delay2)));
    %平均延迟
    time_delay=((time_delay1+time_delay2)/2)*(1/currFs)*10e8;

end

互相关的计算先由MATLAB实现，在由MATLAB Coder转成C++代码。

该互相关代码，考虑了：

1、两路IQ数据采样频率不相同，无法直接做互相关

2、两路IQ数据采样频率过低，最终求得的时差精度低

3、重采样后数据长度过长、或长度不统一

等重采样情况。

源码链接：