使用 SyslabFunction 模块的雷达跟踪

---

模型路径为SyslabWorkspace.Examples.Demo_SyslabFunction_RadarTracking.RadarTracking。

此示例说明如何将扩展 Kalman 滤波器与 SyslabFunction 模块结合使用，以根据雷达测量估计位置。扩展 Kalman 滤波器在SyslabFunction中实现。

Sysplorer 仿真结束后，在 Syslab 中对仿真结果进行分析，系统通过三个图显示以下信息：实际轨迹与估计轨迹的比较；范围的估计残差；以及实际位置、测量位置和估计位置。

其中，Julia 全局配置代码为：

using LinearAlgebra

# 定义全局变量,命名以 g_ 为前缀
g_P = zeros(4, 4)
g_xhat = [0.001; 0.01; 0.001; 400;;] # 4x1矩阵

扩展 Kalman 滤波函数的实现代码为：

function EXTKALMAN(meas, deltat, time)
    # 声明全局变量
    global g_P
    global g_xhat

    # Initialization
    residual = []
    xhatOut = []

    # Radar update time deltat is inherited from model workspace
    # 1. Compute Phi, Q, and R 
    Phi = [1 deltat 0 0; 0 1 0 0; 0 0 1 deltat; 0 0 0 1]
    Q = Diagonal([0, 0.005, 0, 0.005]) # 对应Matlab的diag
    R = Diagonal([300^2, 0.001^2])

    # 2. Propagate the covariance matrix:
    g_P = Phi * g_P * Phi' .+ Q

    # 3. Propagate the track estimate::
    g_xhat = Phi * g_xhat

    # 4 a). Compute observation estimates:
    Rangehat = sqrt(g_xhat[1]^2 + g_xhat[3]^2)
    Bearinghat = atan(g_xhat[3], g_xhat[1]) # 对应Matlab的atan2

    # 4 b). Compute observation vector y and linearized measurement matrix M 
    yhat = [Rangehat
        Bearinghat]
    M = [cos(Bearinghat) 0 sin(Bearinghat) 0
        -sin(Bearinghat)/Rangehat 0 cos(Bearinghat)/Rangehat 0]

    # 4 c).  Compute residual (Estimation Error)
    residual = meas .- yhat

    # 5. Compute Kalman Gain:
    W = g_P * M' * inv(M * g_P * M' .+ R)

    # 6. Update estimate
    g_xhat = g_xhat .+ W * residual

    # 7. Update Covariance Matrix
    g_P = (eye(4) .- W * M) * g_P * (eye(4) .- W * M)' .+ W * R * W'

    xhatOut = g_xhat

    # return residual, xhatOut
    return residual, xhatOut
end

function eye(n::Integer)
    return Int.(Matrix(I(n)))    
end

仿真结束后，系统将通过 ToWorkspace 组件将实际位置、测量位置、估计位置等仿真结果发送到 Syslab 工作区中，在 Syslab 中使用如下代码进行绘图分析：

using TyPlot
#从Sysplorer获取卡尔曼滤波结果
residual = out.residual
X_hat = out.xhatOut
XYCoords = out.XYCoords
PolarCoords = out.PolarCoords
Measurement_noise = out.Measurement_noise
deltat = 1

# 绘图1
figure(figsize=[11.25, 8.1])

margin = [ 0.075, 0.075, -0.1, -0.1 ];
pos = [ 0, 0.5, 0.33, 0.5 ];
axp = pos + margin;
ax = subplot(2, 2, 1, position=axp, projection="polar");
h = polarplot(ax, PolarCoords[:,2] .- Measurement_noise[:,2],
      PolarCoords[:,1] .- Measurement_noise[:,1], "r", linewidth=0.5)
ax.set_yticks([20000,40000])

hold("on")
rangehat = sqrt.(X_hat[:,1].^2 .+ X_hat[:,3].^2);
bearinghat = atan.(X_hat[:,3], X_hat[:,1]);
polarplot(ax, bearinghat,rangehat, color=[0,1,0],linewidth=0.5);
annotation("textbox", [0.1056,0.5378,0.1714,0.0584],
  string="Actual Trajectory (red)\nEstimated Trajectory (green)",
  fitboxtotext="on", edgecolor="none", ha="center")

#绘图2
margin = [ 0.08, 0.075, -0.05, -0.1 ];
pos = [ 0, 0, 0.33, 0.5 ];
axp = pos .+ margin;
ax = subplot(2,2,2, position=axp);
plot(ax, residual[:,1], linewidth=0.5);
grid("on");
xlim([0 length(residual[:,1])]);
xlabel(ax, "Number of Measurements");
ylabel(ax, "Range Estimate Error - Feet")
title(ax, "Estimation Residual for Range")

#绘图3
XYMeas = [PolarCoords[:,1].*cos.(PolarCoords[:,2]) PolarCoords[:,1].*sin.(PolarCoords[:,2])];
numTSteps = size(XYCoords)[1];
t_full    = 0.1 .* (0:numTSteps-1)';
t_hat     = (0:deltat:t_full[end])';

margin = [ 0.1, 0.05, -0.12, -0.1 ];
pos = [ 0.33, 0.5, 0.66, 0.5 ];
axp = pos .+ margin;
axesH = subplot(2,2,3, position=axp);
plot(axesH, t_full, XYCoords[:,2],"r", linewidth=0.5);
grid("on");
hold("on");
plot(axesH, t_full,XYMeas[:,2], color=[0,1,0]);
plot(axesH, t_hat,X_hat[:,3],"b");
title(axesH, "E-W Position");
legend(axesH, ["Actual","Meas","Est"],loc="northwest");
hold("off")

#绘图4
margin = [ 0.1, 0.075, -0.12, -0.1 ];
pos = [ 0.33, 0, 0.66, 0.5 ];
axp = pos .+ margin;
axesH = subplot(2,2,4, position=axp);
plot(axesH, t_full,XYCoords[:,1],"r");
grid("on");
hold("on");
plot(axesH, t_full,XYMeas[:,1],color=[0,1,0]);
plot(axesH, t_hat,X_hat[:,1],"b");
xlabel(axesH, "Time (sec)");
title(axesH, "N-S Position");
hold("off")


plot(t_full,XYCoords[:,1],"r");
grid("on");
hold("on");
plot(t_full,XYMeas[:,1],color=[0,1,0]);
plot(t_hat,X_hat[:,1],"b");
xlabel("Time (sec)");
title("N-S Position");
hold("off")

绘图结果为：