仅测角系统跟踪MATLAB实现，在修正椭圆坐标系MSC下的稳定跟踪算法-EW帮帮网

仅测角系统无法获取传感器与目标之间的距离信息，仅能得到目标在传感器坐标系中的观测角度信息，存在距离不确定性。传统的笛卡尔坐标下，无法实现对目标的稳定跟踪。而在修正的椭圆坐标系下，通过构造新的状态变量，构建匀速运动模型在极坐标系下的非线性转移过程，并利用极坐标系下的测角信息线性观测，能够实现在仅测角系统下的目标稳定跟踪。

运动参数的模糊性如下图所示：

图1 状态不确定性示意图

从图中可以看到，传感器的量测值（即方位&俯仰）可对应无数个目标位置，即不满足目标与量测一一对应关系，无法确定目标真实的运动状态。

参考IEEE Transactions on Aerospace and Electronic Systems期刊论文：

《Heterogeneous Track-to-Track Fusion in 3-D Using IRST Sensor and Air MTI Radar》

一、传感器观测几何

图2 观测几何

二、目标的CV运动方程

假设目标服从匀速直线CV运动，笛卡尔坐标系下运动方程是线性的，其状态向量与运动方程如下：

$\begin{array}{l} \mathbf{x}_{k}^{\mathrm{t}}:=\left[\begin{array}{lllllll} x_{k}^{\mathrm{t}} & y_{k}^{\mathrm{t}} & z_{k}^{\mathrm{t}} & \dot{x}_{k}^{\mathrm{t}} & \dot{y}_{k}^{\mathrm{t}} & \dot{z}_{k}^{\mathrm{t}} \end{array}\right]^{\prime} \end{array}$

$\mathbf{x}_{k}^{\mathrm{t}}=\mathbf{F}_{k-1} \mathbf{x}_{k-1}^{\mathrm{t}}+\mathbf{w}_{k-1}$

其中， $\mathbf{F}_{k-1}$ 是目标的运动转移矩阵， $\mathbf{w}_{k-1}$ 是一个零均值的高斯白噪声，其协方差是 $\mathbf{Q}_{k-1}$ 。具体形式是：

$\mathbf{F}_{k-1}=\left[\begin{array}{cc} \mathbf{I}_{3} & \mathbf{I}_{3} \Delta_{k} \\ \mathbf{0}_{3 \times 3} & \mathbf{I}_{3} \end{array}\right]$

$\Delta_{k}:=t_{k}-t_{k-1}$

$\mathbf{Q}_{k-1}=\left[\begin{array}{cc} \operatorname{diag}(\mathbf{q}) \Delta_{k}^{3} / 3 & \operatorname{diag}(\mathbf{q}) \Delta_{k}^{2} / 2 \\ \operatorname{diag}(\mathbf{q}) \Delta_{k}^{2} / 2 & \operatorname{diag}(\mathbf{q}) \Delta_{k} \end{array}\right]$

为了实现仅测角系统的稳定跟踪，构建修正椭圆坐标系下的状态向量如下：

$\begin{aligned} \boldsymbol{\xi}_{k} & :=\left[\begin{array}{llllll} \xi_{k, 1} & \xi_{k, 2} & \xi_{k, 3} & \xi_{k, 4} & \xi_{k, 5} & \xi_{k, 6} \end{array}\right]^{\prime} \\ & =\left[\begin{array}{lllll} \omega_{k} & \dot{\epsilon}_{k} & \dot{\zeta}_{k} & \beta_{k} & \epsilon_{k} \end{array} \frac{1}{r_{k}}\right]^{\prime} \end{aligned}$

$\mathbf{x}_{k}^{\mathrm{t}}$ 与 $\boldsymbol{\xi}_{k}$ 之间存在一一对应的关系，具体如下：

$\mathbf{f}_{\mathrm{C}}^{\mathrm{MSC}}(\mathbf{x}):=\left[\begin{array}{c} (y \dot{x}-x \dot{y}) /(\rho r) \\ {\left[\dot{z} \rho^{2}-z(x \dot{x}+y \dot{y})\right] /\left(\rho r^{2}\right)} \\ (x \dot{x}+y \dot{y}+z \dot{z}) / r^{2} \\ \tan ^{-1}(x, y) \\ \arctan (z / \rho) \\ 1 / r \end{array}\right]$

其中， $r=\sqrt{x^{2}+y^{2}+z^{2}}, \quad \rho=\sqrt{x^{2}+y^{2}}$ 。

$\mathbf{f}_{\mathrm{MSC}}^{\mathrm{C}}(\boldsymbol{\xi}):=\frac{1}{\xi_{6}}\left[\begin{array}{c} \sin \xi_{4} \cos \xi_{5} \\ \cos \xi_{4} \cos \xi_{5} \\ \sin \xi_{5} \\ \sin \xi_{4}\left(\xi_{3} \cos \xi_{5}-\xi_{2} \sin \xi_{5}\right)+\xi_{1} \cos \xi_{4} \\ \cos \xi_{4}\left(\xi_{3} \cos \xi_{5}-\xi_{2} \sin \xi_{5}\right)-\xi_{1} \sin \xi_{4} \\ \xi_{2} \cos \xi_{5}+\xi_{3} \sin \xi_{5} \end{array}\right]$

此时，联合上述状态变换以及笛卡尔坐标系下的线性运动方程，可以得到修正的椭圆坐标系下的非线性运动方程：

$\boldsymbol{\xi}_{k}=\mathbf{f}_{\mathrm{C}}^{\mathrm{MSC}}\left(\mathbf{F}_{k-1} \mathbf{f}_{\mathrm{MSC}}^{\mathrm{C}}\left(\boldsymbol{\xi}_{k-1}\right)+\mathbf{w}_{k-1}-\mathbf{U}_{k-1}\right) .$

注意：此时运动模型是非线性的，需要采用无迹变换进行预测。

三、线性观测模型

仅测角系统的量测方程（方位角&俯仰角）以及对应的观测协方差矩阵可以表示为：

$\mathbf{z}_{k}^{\mathrm{P}}=\mathbf{H} \xi_{k}+\mathbf{n}_{k}^{\mathrm{P}}$

$\begin{aligned} \mathbf{H} & :=\left[\begin{array}{llllll} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \end{array}\right] \\ \mathbf{n}_{k}^{\mathrm{P}} & \sim \mathcal{N}\left(\mathbf{n}_{k}^{\mathrm{P}} ; \mathbf{0}, \mathbf{R}^{\mathrm{P}}\right) \\ \mathbf{R}^{\mathrm{P}} & :=\operatorname{diag}\left(\sigma_{\beta_{2}}^{2}, \sigma_{\epsilon_{2}}^{2}\right) \end{aligned}$

注意：此时量测模型是线性的，可以采用KF的更新过程。

四、仿真实验结果

构建如下图3中的观测场景：

图3 观测场景

算法的滤波结果如下：

图4 三个方向的实际轨迹与跟踪轨迹

图5 位置收敛曲线

图6 速度收敛曲线

从上述结果可以看出，尽管仅测角系统没有测距信息，但是仍然能够实现目标的稳定跟踪，其跟踪误差逐步收敛。

部分主函数与子函数如下，如有代码问题，加UltraNextYJ交流。

for i_mc = 1:MC

    x_cart0 = [5500, -50, 700, 30, 2300, -20]';
    P_cart0 = diag([100^2,2^2,100^2,2^2,100^2,2^2]);

    %% 不同坐标系状态向量的转换
    [x_msc0] = fun_state_cart2msc(x_cart0);

    %% 不同坐标系协方差矩阵的转换
    [P_msc0] = fun_CT_cov_cart2msc(x_cart0, P_cart0);

    % 滤波状态初始化
    xk_msc_CKF = x_msc0;
    Pk_msc_CKF = P_msc0; 

    x0 = mvnrnd(x_msc0, P_msc0);   % 初始状态
    x_msc = x0';

    for k = 1:N
        %%%%%%%%%%%%%%%%%%%%% 目标模型和测角量测模型 %%%%%%%%%%%%%%%%%%%
        % 目标模型CV
        w = sqrtm(Qk) * randn(6,1);    % 过程噪声（开方得到标准差）
        % 极坐标系下的运动方程，预测仅仅用于存储真实值
        x_msc = fun_state_cart2msc(Fk * fun_state_msc2cart(x_msc) + Gk * w); 
        x_cart = fun_state_msc2cart(x_msc);     
        sV(:,k,i_mc) = x_cart;

        % 红外仅测角的量测模型
        v = normrnd(v_mu, [sigmaAz; sigmaEl]);
        [az,el] = MeaOnlyAngle(x_cart, xp); 
        AzMea = az + v(1);
        ElMea = el + v(2);
        rV(:,k,i_mc) = [AzMea, ElMea]';

        %%%%%%%%%%% MSC-CKF %%%%%%%%%

        [xk_msc_CKF, Pk_msc_CKF] = fun_MSC_CKF(xk_msc_CKF,Pk_msc_CKF,Fk,Gk,Qk,rV(:,k,i_mc),Rk,H);

        %%%%%%%%%%%%%% end filter 
    end

end


function [x_cart] = fun_state_msc2cart(x_mpc)
    % 提取 MPC 下的分量
    xi1 = x_mpc(1);
    xi2 = x_mpc(2);
    xi3 = x_mpc(3);
    xi4 = x_mpc(4);   % 对应实际的方位角（此处方位角的定义是不一样的）
    xi5 = x_mpc(5);   % 对应实际的俯仰角 
    xi6 = x_mpc(6);   % 对应距离的倒数

    % 计算速度分量转换所需的角度余弦和正弦值
    cosXi4 = cos(xi4);
    sinXi4 = sin(xi4);
    cosXi5 = cos(xi5);
    sinXi5 = sin(xi5);

    % 计算笛卡尔坐标系下的位置分量
    xPos = sinXi4 * cosXi5 / xi6;
    yPos = cosXi4 * cosXi5 / xi6;
    zPos = sinXi5 / xi6;
    
    % 计算笛卡尔坐标系下的速度分量
    xDot = (sinXi4*(xi3*cosXi5 - xi2*sinXi5) + xi1*cosXi4) / xi6;
    yDot = (cosXi4*(xi3*cosXi5 - xi2*sinXi5) - xi1*sinXi4) / xi6;
    zDot = (xi2*cosXi5 + xi3*sinXi5) / xi6;
    
    % 构造笛卡尔坐标系状态向量
    x_cart = [xPos; xDot; yPos; yDot; zPos; zDot];
end

function [x_msc] = fun_state_cart2msc(x_cart)
    
    % 提取位置分量
    xPos = x_cart(1);
    yPos = x_cart(3);
    zPos = x_cart(5);
    
    % 提取速度分量
    xDot = x_cart(2);
    yDot = x_cart(4);
    zDot = x_cart(6);
    
    % 计算 rho 和 r
    rho = sqrt(xPos^2 + yPos^2);
    r = sqrt(xPos^2 + yPos^2 + zPos^2);
    
    % 计算 MSCS 下的位置部分
    az = atan2(xPos,yPos);    % 等价于 arctan(x/y)   % 此处的定义并不相同，两个角度是互余的
    el = atan2(zPos, rho);    % 等价于 arctan(z/rho)
    r_inv = 1 / r;
    
    % 计算 MSCS 下的速度部分
    % 计算第一速度分量 (对应 xi_1)
    azRate = (yPos * xDot - xPos * yDot) ./ (rho * r);
    
    % 计算第二速度分量 (对应 xi_2)
    elRate = (zDot * rho^2 - zPos * (xPos * xDot + yPos * yDot)) ./ (rho * r^2);
    
    % 计算第三速度分量 (对应 xi_3)
    rDot = (xPos * xDot + yPos * yDot + zPos * zDot) ./ (r^2);
    
    % 构造 MSCS 状态向量
    x_msc = [azRate; elRate; rDot; az; el; r_inv];
end

仅测角系统跟踪MATLAB实现，在修正椭圆坐标系MSC下的稳定跟踪算法

网站公告

今日签到

热门文章

最新发布