基于扩展卡尔曼滤波器的绕回测量值状态估计

此示例使用:

Simulink Simulink

本示例演示了如何使用扩展卡尔曼滤波算法，对涉及环绕角度测量值的三维跟踪进行非线性状态估计。在目标跟踪中，传感器通常采用球坐标系，以方位角、距离和仰角来报告目标的位置。该数据集中的角度测量值在一定范围内报告。例如，方位角的报告范围为 $- 18 0^{\circ}$ 至 $18 0^{\circ}$ 或 $0^{\circ}$ 至 $36 0^{\circ}$ 。然而，跟踪通常是在矩形坐标系中进行的，这需要使用一种非线性滤波器，该滤波器能够处理将球坐标系测量值转换为矩形坐标系状态所需的非线性变换。

本示例使用恒定速度模型来追踪目标的三维位置和速度。采用扩展卡尔曼滤波器作为非线性滤波器，用于跟踪该对象的状态。在此跟踪演练中，该滤波器使用方位角 ( $ψ$ )、距离和仰角 ( $ϕ$ ) 的读数作为测量数据。

本示例中的恒速三维物体模型由三个坐标轴上的位置和速度组成，具体如下：

$S t a t e s = [\begin{array}{c} x \\ x_{}^{˙} \\ y \\ y_{}^{˙} \\ z \\ z_{}^{˙} \end{array}] = [\begin{array}{c} x \\ v_{x} \\ y \\ v_{y} \\ z \\ v_{z} \end{array}]$

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

$\begin{array}{l} R a n g e = \sqrt{x^{2} + y^{2} + z^{2}} \\ E l e v a t i o n (ϕ) = \sin^{- 1} (\frac{z}{R a n g e}) \\ A z i m u t h (ψ) = \tan^{- 1} (\frac{y}{x}) \end{array}$

计算实际测量值与预期测量值之间的残差，是滤波过程中至关重要的一步。如果不采取必要的预防措施，位于绕回边界附近的物体的残余值可能会很大。这会导致滤波器偏离精确状态，从而导致状态估计不准确。本示例演示了如何通过测量值绕回处理此类情况。使用 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));

运行滤波器，并在三维图上可视化状态收敛情况。

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

Figure contains an axes object. The axes object with title No measurement wrapping, xlabel x, ylabel y contains 35 objects of type line. One or more of the lines displays its values using only markers

状态估计 - 使用测量值绕回

现在，启用测量值绕回功能并运行滤波器。在三维图上可视化状态收敛情况。

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

Figure contains an axes object. The axes object with title With measurement wrapping contains 35 objects of type line. One or more of the lines displays its values using only markers

从上图可以看出，这三个坐标轴的刻度范围远小于未进行测量绕回处理情况下的三维图。这表明，启用测量值绕回后，收敛性更好。

验证

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

figure()
subplot(3,1,1)
plot(xEstNoWrap(:,1) - xTrue(1));
hold on
plot(xEstWithWrap(:,1) - xTrue(1));
ylim([-500 1200])
title('e_x');
hold off

subplot(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')

Figure contains 3 axes objects and another object of type subplottext. Axes object 1 with title e indexOf x baseline contains 2 objects of type line. Axes object 2 with title e indexOf y baseline contains 2 objects of type line. Axes object 3 with title e indexOf z baseline contains 2 objects of type line. These objects represent No Wrapping, With Wrapping.

请注意，在未进行测量值绕回的情况下，三个坐标轴上的误差都相对大得多。

总结

本示例演示了如何使用 Extended Kalman Filter 模块对存在和不存在测量值绕回的非线性系统进行状态估计。从三维图和误差图可以看出，启用绕回功能的状态估计能提供更准确的状态估计结果。

支持函数

状态转移函数

Extended Kalman Filter 模块需要一个函数，用于描述状态从一个时间步到下一个时间步的演变过程。这通常被称为状态转移函数。例如，在此示例中，stateTransitionFcn.m 中包含的状态转移函数的内容如下：

function x = stateTransitionFcn(x)
   dt = 3;
   A = [1 dt 0 0  0 0;
        0 1  0 0  0 0;
        0 0  1 dt 0 0;
        0 0  0 1  0 0;
        0 0  0 0  1 dt;
        0 0  0 0  0  1];
   x = A*x;
end

测量函数

Extended Kalman Filter 模块还需要一个函数，用于描述模型状态与传感器测量值之间的关系。这是测量函数。在此示例中，measurementFcn.m 中包含的测量函数内容如下：

function [y, bounds] = measurementFcn(x)
   xPos = x(1);
   yPos = x(3);
   zPos = x(5);

   % Range
   r = sqrt(xPos^2 + yPos^2 + zPos^2);
   % Azimuth
   psi = atan2d(yPos,xPos);
   % Elevation
   phi = asind(zPos/r);
   % Combined measurement
   y = [r ; psi; phi];
   bounds = [-inf inf;-180 180; -90 90]; 
end

另请参阅

Extended Kalman Filter