扩展卡尔曼滤波算法——基本原理（附MATLAB程序）

扩展卡尔曼滤波算法——基本原理（附MATLAB程序）

引用

CSDN

1.

https://blog.csdn.net/qq_35623594/article/details/141278449

扩展卡尔曼滤波（Extended Kalman Filter, EKF）是卡尔曼滤波（Kalman Filter, KF）的一个扩展，主要用于处理非线性系统中的状态估计问题。下面是对扩展卡尔曼滤波的详细介绍：

1. 基本概念

卡尔曼滤波是一种递归的滤波算法，用于估计线性动态系统的状态。然而，许多实际系统是非线性的，因此需要扩展卡尔曼滤波来处理这种情况。

2. 非线性系统的挑战

在非线性系统中，状态转移方程和观测方程通常是非线性的。标准的卡尔曼滤波假设这些方程是线性的，因此它不能直接用于非线性系统。扩展卡尔曼滤波通过线性化这些非线性方程来解决这个问题。

3. 算法步骤

3.1 预测步骤

在每个时间步，EKF首先通过状态转移方程来预测当前时刻的状态和协方差矩阵。

状态预测：

其中，

• $\hat{x}_{k|k-1}$ 是状态的预测值，
• $f$ 是非线性的状态转移函数，
• $u_k$ 是控制输入。

协方差预测：

其中，

• $P_{k|k-1}$ 是状态协方差矩阵的预测值，
• $F_k$ 是状态转移函数$f$关于状态的雅可比矩阵，
• $Q_k$ 是过程噪声协方差矩阵。

3.2 更新步骤

然后，根据观测值来更新状态和协方差矩阵。

计算卡尔曼增益：

其中，

• $K_k$ 是卡尔曼增益，
• $H_k$ 是观测函数$h$关于状态的雅可比矩阵，
• $R_k$ 是观测噪声协方差矩阵。

更新状态估计：

其中，

• $\hat{x}_k$ 是更新后的状态估计值，
• $z_k$ 是实际观测值，
• $h$ 是非线性的观测函数。

更新协方差矩阵：

其中，$I$ 是单位矩阵。

4. 关键点

• 线性化：EKF通过计算非线性函数的雅可比矩阵来近似线性化状态转移和观测方程。这是EKF的核心，也是其处理非线性问题的关键。
• 计算复杂性：EKF的计算复杂性相较于线性卡尔曼滤波更高，特别是在状态和观测维度较大时，因为需要计算雅可比矩阵并进行矩阵运算。

5. 应用

扩展卡尔曼滤波广泛应用于机器人定位、航天飞行、自动驾驶、无人机导航等需要处理非线性系统状态估计的领域。

6. MATLAB仿真程序

1. 设置系统模型

% 系统参数
n = 4; % 状态维度
m = 2; % 观测维度

% 初始状态
x0 = [0; 0; 1; 1]; % [位置x, 速度x, 位置y, 速度y]
P0 = eye(n); % 初始协方差矩阵

% 系统噪声和观测噪声
Q = 0.1 * eye(n); % 过程噪声协方差
R = 0.1 * eye(m); % 观测噪声协方差

% 时间参数
dt = 0.1; % 时间步长
T = 10; % 总时间
time = 0:dt:T; % 时间向量

% 初始化
x_est = zeros(n, length(time)); % 状态估计
x_true = zeros(n, length(time)); % 真实状态
y_meas = zeros(m, length(time)); % 观测数据

% 初始状态
x_true(:,1) = x0;

% 生成真实状态和观测数据
for t = 2:length(time)
    % 状态转移
    x_true(:,t) = [x_true(1,t-1) + x_true(3,t-1)*dt + 0.5*randn; % x位置
                   x_true(2,t-1) + randn; % x速度
                   x_true(3,t-1) + x_true(4,t-1)*dt + 0.5*randn; % y位置
                   x_true(4,t-1) + randn]; % y速度

    % 生成观测数据
    y_meas(:,t) = [x_true(1,t)^2; x_true(3,t)^2] + sqrt(R) * randn(m,1);
end

2. 扩展卡尔曼滤波器实现

% 初始化
x = x0; % 初始状态估计
P = P0; % 初始协方差矩阵

% 扩展卡尔曼滤波器
for t = 2:length(time)
    % 状态转移函数
    f = @(x) [x(1) + x(3)*dt + 0.5*randn; % x位置
              x(2) + randn; % x速度
              x(3) + x(4)*dt + 0.5*randn; % y位置
              x(4) + randn]; % y速度

    % 观测函数
    h = @(x) [x(1)^2; x(3)^2];

    % 计算雅可比矩阵
    F = eye(n) + [0, 0, dt, 0; % 状态转移的雅可比矩阵
                  0, 1, 0, 0;
                  0, 0, 0, dt;
                  0, 0, 0, 1];
    H = [2*x(1), 0, 0, 0; % 观测函数的雅可比矩阵
         0, 0, 2*x(3), 0];

    % 预测步骤
    x_pred = f(x); % 预测状态
    P_pred = F * P * F' + Q; % 预测协方差矩阵

    % 更新步骤
    y_pred = h(x_pred); % 预测观测
    K = P_pred * H' / (H * P_pred * H' + R); % 卡尔曼增益
    x = x_pred + K * (y_meas(:,t) - y_pred); % 更新状态估计
    P = (eye(n) - K * H) * P_pred; % 更新协方差矩阵

    % 记录状态估计
    x_est(:,t) = x;
end

3. 绘制结果

% 绘制真实状态、观测数据和估计状态
figure;
subplot(2,1,1);
plot(time, x_true(1,:), 'g', time, x_est(1,:), 'b--');
legend('True Position X', 'Estimated Position X');
xlabel('Time (s)');
ylabel('Position X');

subplot(2,1,2);
plot(time, x_true(3,:), 'g', time, x_est(3,:), 'b--');
legend('True Position Y', 'Estimated Position Y');
xlabel('Time (s)');
ylabel('Position Y');

说明：

• 系统模型：
• $x_0$：初始状态。
• $P_0$：初始协方差矩阵。
• $Q$：过程噪声协方差矩阵。
• $R$：观测噪声协方差矩阵。
• $f$ 和 $h$：状态转移和观测函数。
• 扩展卡尔曼滤波器实现：
• 状态转移函数 $f$ 和观测函数 $h$：定义系统的非线性状态转移和观测模型。
• 雅可比矩阵 $F$ 和 $H$：线性化非线性模型，用于计算预测和更新步骤。
• 预测步骤：通过状态转移函数预测下一个状态。
• 更新步骤：利用观测数据更新状态估计和协方差矩阵。
• 结果绘制：
• 绘制真实状态和扩展卡尔曼滤波的状态估计，以便比较滤波器的效果。