使用扩展卡尔曼滤波器进行包裹测量的状态估计

此示例说明如何使用扩展卡尔曼滤波器算法对涉及圆形包裹角度测量的 3D 跟踪进行非线性状态估计。对于目标跟踪，传感器通常采用球形框架来报告物体的方位角、距离和仰角位置。该组的角度测量值在一定范围内报告。例如，报告的方位角范围为- 180∘ 到180∘或者 0∘到360∘。然而，跟踪通常在矩形框架中完成，这需要使用非线性滤波器来处理将球形测量值转换为矩形状态所需的非线性变换。

此示例使用恒速模型来跟踪目标的三维位置和速度。扩展卡尔曼滤波器用作非线性滤波器来跟踪该对象的状态。该过滤器使用方位角（ψ)、范围和高度 (φ) 读数作为跟踪练习期间的测量值。

本例的恒速 3D 对象模型由三个轴上的位置和速度组成，如下所示：

距离、方位角和仰角由下式给出：

计算实际测量值和预期测量值之间的残差是过滤过程中的重要步骤。如果不采取必要的预防措施，对于位于缠绕边界附近的物体来说，残差值可能会很大。这会导致滤波器偏离准确的状态，从而导致状态估计不准确。此示例演示如何使用测量包装来处理此类情况。 Simulink® 模型用于演示使用和不使用测量环绕时跟踪状态的差异。

状态估计 - 无测量包裹

首先提供您的初始状态猜测并指定模型名称。

rng(0)
initialStateGuess = [-100 0 0 0 0 0]';
modelName = 'modelEKFWrappedMeasurements';

根据噪声协方差生成测量值，如下所示：

dt = 3;
tSpan = 0:dt:100;
yTrue = zeros(length(tSpan),3);
yMeas = zeros(size(yTrue));
noiseCovariance = diag([20 0.5 0.5]);
xCurrent = initialStateGuess;for i = 1:numel(tSpan)xTrue = stateTransitionFcn(xCurrent);yTrue(i,:) = (measurementFcn(xTrue))';yMeas(i,:) = yTrue(i,:) + (chol(noiseCovariance)*randn(3,1))';xCurrent = xTrue;
end

模拟模型。

out = sim(modelName);
xEstNoWrap = zeros(numel(tSpan),length(initialStateGuess));
xEstWithWrap = zeros(numel(tSpan),length(initialStateGuess));

运行过滤器并在 3D 图上可视化状态收敛。

figure()
plot3(initialStateGuess(1), initialStateGuess(3), initialStateGuess(5),'ro','MarkerFaceColor','r','MarkerSize',9);
grid
hold on;% Plot options
xlabel('x');
ylabel('y');
zlabel('z');
title('No measurement wrapping');% Run filter
for i = 1:numel(tSpan)xEst = out.noWrapOut(i,:);plot3(xEst(1),xEst(3),xEst(5),'bo','MarkerFaceColor','b');   hold on;xEstNoWrap(i,:) = xEst;
end
hold off

状态估计 - 带测量包裹

现在，启用测量包装并运行过滤器。在 3D 图上可视化状态收敛。

figure()
% Run filter
for i = 1:numel(tSpan)xEst = out.wrapOut(i,:);plot3(xEst(1),xEst(3),xEst(5),'bo','MarkerFaceColor','b');   hold on;xEstWithWrap(i,:) = xEst;
end
plot3(initialStateGuess(1), initialStateGuess(3), initialStateGuess(5),'ro','MarkerFaceColor','r','MarkerSize',9);
grid
title('With measurement wrapping');
hold off

从上图中可以看出，三个轴的比例比没有测量环绕的情况下的 3D 图小得多。这表示启用测量环绕时有更好的收敛性。

验证

在同一图上比较两种情况的估计误差。

figure()
subplot(3,1,1)
plot(xEstNoWrap(:,1) - xTrue(1));
hold on
plot(xEstWithWrap(:,1) - xTrue(1));
ylim([-500 1200])
title('e_x');
hold offsubplot(3,1,2)
plot(xEstNoWrap(:,3) - xTrue(3));
hold on
plot(xEstWithWrap(:,3) - xTrue(3));
ylim([-2000 1000])
hold off
title('e_y');subplot(3,1,3)
plot(xEstNoWrap(:,5) - xTrue(5));
hold on
plot(xEstWithWrap(:,5) - xTrue(5));
ylim([-200 200])
hold off
title('e_z');sgtitle('Estimation Error Plot')
legend('No Wrapping', 'With Wrapping','location','best')