<-- Home |--matlab

|欢迎您，第位读者！|

Unscented Kalman Filter in Matlab无迹卡尔曼滤波

无迹卡尔曼滤波(Unscented Kalman Filter, UKF)

无迹卡尔曼滤波(Unscented Kalman Filter, UKF)是一种基于卡尔曼滤波的非线性状态估计方法，它通过一种称为无迹变换(Unscented Transformation)的方法，将非线性系统的状态估计问题转化为线性系统的状态估计问题，然后使用卡尔曼滤波器进行状态估计。

这是一种在线方法，也就是说，它会连续地对系统状态进行估计，而不是像批处理方法那样，一次性处理所有的数据。

要定义一个UKF，我们需要定义如下两个函数：

  1. 状态转移函数 $f(x, u, t)$
  2. 观测函数 $h(x)$

其中，$x$ 是状态向量，$u$ 是控制向量，$t$ 是时间，$f$ 是状态转移函数，$h$ 是观测函数。

提供这两个函数外加噪声的信息，既可以创建一个UKF对象。其后，就可以通过predict来预测下个时间步长的状态；在拿到实时数据后，通过correct函数来校正状态。

系统的框图如上所示。非线性系统的输入为 $u$，系统噪声为 $w$，系统状态为 $x$，观测值为 $y$， 观测噪声为 $v$。UKF 通过输入 $u$ 和观测值 $y$ 来估计系统状态 $\hat{x}$。

van der Pol Oscillator

这里我们以 van der Pol Oscillator 为例，来演示如何使用 UKF 进行状态估计。

首先，我们给出 van der Pol Oscillator 的一般形式：

$$ \ddot{x} - \mu (1 - x^2) \dot{x} + x = 0 $$

其中，$\mu$ 是 van der Pol Oscillator 的参数。

这个方程可以被转化为两个一阶微分方程：

\begin{cases} \dot{x_1} &= x_2 \\ \dot{x_2} &= \mu (1 - x_1^2) x_2 - x_1 \\ \end{cases}

采用ODE45求解这个方程，我们可以得到 van der Pol Oscillator 的状态变化。

 1T = 0.05; % [s] Filter sample time
 2timeVector = 0:T:5;
 3[t, xTrue] = ode45(@vdp1, timeVector, [2; 0]);
 4
 5figure;
 6plot(t, xTrue, 'LineWidth', 2);
 7xlabel('Time [s]');
 8ylabel('States');
 9legend('x_1', 'x_2', 'Location', 'Best');
10title('Van der Pol Oscillator');
11grid on;
12
13exportgraphics(gcf, '../matlab-img/vdp1Ode.png', 'Resolution', 100);

观测

假设我们对 van der Pol Oscillator 的状态进行观测，但是观测值是带有噪声的。

\begin{cases} R = 0.2 \\ y_1 = x_1 * (1 + \mathcal{N}(0, R)) \\ \end{cases}

 1R = 0.2;
 2
 3rng(1); % For reproducibility
 4yTrue = xTrue(:, 1);
 5yMeas = yTrue .* (1 + sqrt(R) * randn(size(yTrue)));
 6
 7figure;
 8plot(t, yTrue, 'LineWidth', 2);
 9hold on;
10plot(t, yMeas, 'LineWidth', 2);
11xlabel('Time [s]');
12ylabel('State: x_1');
13legend('True', 'Measured', 'Location', 'Best');
14title('Van der Pol Oscillator Observations of x_1');
15
16exportgraphics(gcf, '../matlab-img/vdp1Observ.png', 'Resolution', 100);

UKF

接下来，我们使用 UKF 对 van der Pol Oscillator 的状态进行估计。

 1% 猜测初始状态
 2initialStateGuess = [2; 0]; % xhat[k|k-1]
 3% 构造 UKF
 4ukf = unscentedKalmanFilter( ...
 5    @vdpStateFcn, ... % State transition function
 6    @vdpMeasurementNonAdditiveNoiseFcn, ... % Measurement function
 7    initialStateGuess, ...
 8    'HasAdditiveMeasurementNoise', false);
 9
10% 设置 UKF 参数
11ukf.MeasurementNoise = R;
12ukf.ProcessNoise = diag([0.02 0.1]);
13
14% 初始化数组存储
15Nsteps = numel(yMeas); % Number of time steps
16xCorrectedUKF = zeros(Nsteps, 2); % Corrected state estimates
17PCorrected = zeros(Nsteps, 2, 2); % Corrected state estimation error covariances
18e = zeros(Nsteps, 1); % Residuals (or innovations)
19
20for k = 1:Nsteps % k时刻
21    % 新息更新
22    e(k) = yMeas(k) - vdpMeasurementFcn(ukf.State); % ukf.State <- x[k|k-1]
23    % 新的输入测量更新状态估计
24    [xCorrectedUKF(k, :), PCorrected(k, :, :)] = correct(ukf, yMeas(k)); % ukf.State <- x[k|k]
25    % 预测下一个状态
26    predict(ukf);
27end
28
29% 绘制结果
30figure();
31h = plot(timeVector, xTrue(:, 1), timeVector, xCorrectedUKF(:, 1), timeVector, yMeas(:), 'LineWidth', 0.5);
32set(h(2), 'LineWidth', 2);
33set(h(1), 'LineWidth', 2);
34
35legend('True', 'UKF estimate', 'Measured', 'Location', 'Best');
36xlabel('Time [s]');
37ylabel('State: x_1');
38title('Van der Pol Oscillator State Estimation with UKF');
39
40exportgraphics(gcf, '../matlab-img/vdp1Ukf.png', 'Resolution', 100);
41
42function x = vdpStateFcn(x)
43    dt = 0.05; % [s] Sample time
44    x = x + vdpStateFcnContinuous(x) * dt;
45end
46
47function dxdt = vdpStateFcnContinuous(x)
48    %取 mu = 1的 van der Pol ODE
49    dxdt = [x(2); (1 - x(1) ^ 2) * x(2) - x(1)];
50end
51
52function yk = vdpMeasurementNonAdditiveNoiseFcn(xk, vk)
53    yk = xk(1) * (1 + vk);
54end

经过 UKF 估计后，我们可以得到 van der Pol Oscillator 的状态估计值。

进一步分析

当然，我们还可以对新息、协方差等进行分析。这里仅仅展示对一个连续的状态进行UKF估计的过程。

参考：Van der Pol Equation: Overview, Derivation, and Examination of Solutions

文章标签

|-->matlab |-->state estimation |-->ukf |-->system identification

GitHub