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

leon625

已于 2024-08-21 11:40:49 修改

阅读量4.5k

点赞数 16

分类专栏：算法类文章标签：算法

于 2024-08-18 10:30:33 首次发布

本文链接：https://blog.csdn.net/qq_35623594/article/details/141293512

版权

算法类专栏收录该内容

21 篇文章

订阅专栏

无迹卡尔曼滤波（Unscented Kalman Filter, UKF）是一种用于处理非线性系统状态估计的递归滤波器。相比于扩展卡尔曼滤波（EKF），UKF在处理非线性问题时通常表现得更为精确和稳健。它通过一种称为“无迹变换”（Unscented Transform, UT）的技术来近似非线性函数，从而避免了对雅可比矩阵的计算，这使得它对非线性系统有更好的适应性。

一、无迹卡尔曼滤波算法的基本原理

UKF的核心思想是通过选择一组称为“sigma点”的离散点来近似系统的状态分布。通过对这些sigma点进行非线性变换，然后计算这些点变换后的均值和协方差，从而得到对系统状态的估计。

主要步骤

1. 生成sigma点

对于状态向量和协方差矩阵，生成一组sigma点。这些sigma点通过以下公式计算：

其中，λ 是缩放参数，n 是状态维度。

2.预测sigma点

将生成的sigma点通过状态转移函数进行非线性变换，得到预测的sigma点。

3. 计算预测均值和协方差

根据预测的sigma点计算预测状态的均值和协方差：

其中，和是sigma点的权重，Q是过程噪声协方差矩阵。

4. 预测测量

将预测的sigma点通过测量函数进行非线性变换，得到预测的测量sigma点：

5. 计算测量均值和协方差

根据预测的测量sigma点计算测量的均值和协方差：

其中，是测量协方差矩阵，是状态与测量的交叉协方差矩阵，R是测量噪声协方差矩阵。

6. 更新

计算卡尔曼增益并更新状态估计和协方差矩阵：

其中，是实际测量值。

二、无迹卡尔曼滤波的优点

处理非线性：UKF比EKF能更好地处理强非线性系统，因为它避免了线性化误差。
不需要计算雅可比矩阵：无迹卡尔曼滤波直接通过sigma点近似非线性函数，简化了计算。
稳定性：在处理高非线性问题时比扩展卡尔曼滤波器更稳定。

三、应用场景

无人驾驶汽车：实时估计车辆状态和环境感知。
机器人控制：复杂机器人运动和位置估计。
飞行控制：航天器和飞行器的状态估计与导航。
金融数据分析：用于预测和风险评估。

无迹卡尔曼滤波器由于其处理非线性问题的能力和较高的估计精度，在许多实际应用中都具有很大的优势。

四、MATLAB仿真程序

1. 设置系统模型

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

% 初始状态
x0 = [0; 0; 1; 1];
P0 = eye(n); % 状态协方差矩阵的初始值

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

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

% 状态转移函数
f = @(x) [x(1) + x(3); x(2) + x(4); x(3); x(4)]; % 非线性状态转移函数

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

2. 无迹卡尔曼滤波器实现

% UKF参数设置
alpha = 1e-3; % 控制sigma点的分布
beta = 2; % 先验知识参数
kappa = 0; % 可选参数，通常设为0

lambda = alpha^2 * (n + kappa) - n; % 计算lambda
c = n + lambda; % 权重常数

% 权重
Wm = [lambda / c; repmat(1 / (2 * c), 2 * n, 1)]; % 预测权重
Wc = [lambda / c + (1 - alpha^2 + beta); repmat(1 / (2 * c), 2 * n, 1)]; % 更新权重

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

% 记录数据
X_est = zeros(n, length(time)); % 状态估计
X_true = zeros(n, length(time)); % 实际状态
Y_meas = zeros(m, length(time)); % 观测数据

% 仿真循环
for t = 1:length(time)
    % 生成sigma点
    sqrt_P = sqrtm(P); % 协方差矩阵的平方根
    X_sig = [x, x + sqrt_P, x - sqrt_P]; % sigma点

    % 状态传播
    X_sig_pred = zeros(n, 2*n + 1);
    for i = 1:2*n + 1
        X_sig_pred(:,i) = f(X_sig(:,i));
    end

    % 计算预测均值和协方差
    x_pred = X_sig_pred * Wm;
    P_pred = Q;
    for i = 1:2*n + 1
        P_pred = P_pred + Wc(i) * (X_sig_pred(:,i) - x_pred) * (X_sig_pred(:,i) - x_pred)';
    end

    % 生成观测sigma点
    Y_sig_pred = zeros(m, 2*n + 1);
    for i = 1:2*n + 1
        Y_sig_pred(:,i) = h(X_sig_pred(:,i));
    end

    % 计算观测均值和协方差
    y_pred = Y_sig_pred * Wm;
    P_yy = R;
    P_xy = zeros(n, m);
    for i = 1:2*n + 1
        P_yy = P_yy + Wc(i) * (Y_sig_pred(:,i) - y_pred) * (Y_sig_pred(:,i) - y_pred)';
        P_xy = P_xy + Wc(i) * (X_sig_pred(:,i) - x_pred) * (Y_sig_pred(:,i) - y_pred)';
    end

    % 计算卡尔曼增益
    K = P_xy / P_yy;

    % 更新状态和协方差矩阵
    z = h(x) + sqrt(R) * randn(m,1); % 模拟观测
    x = x_pred + K * (z - y_pred);
    P = P_pred - K * P_yy * K';

    % 记录数据
    X_est(:,t) = x;
    X_true(:,t) = f(x) + sqrt(Q) * randn(n,1); % 模拟真实状态
    Y_meas(:,t) = h(X_true(:,t)) + sqrt(R) * randn(m,1); % 模拟观测
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(2,:), 'g', time, X_est(2,:), 'b--');
legend('True Position Y', 'Estimated Position Y');
xlabel('Time (s)');
ylabel('Position Y');