一文详解自适应无迹卡尔曼滤波（AUKF）

Luminosity_azur

已于 2023-06-14 12:51:42 修改

阅读量5.8k

点赞数 6

分类专栏：卡尔曼滤波文章标签：人工智能自动驾驶 matlab

于 2023-06-04 02:41:51 首次发布

本文链接：https://blog.csdn.net/weixin_45766278/article/details/131028011

版权

卡尔曼滤波专栏收录该内容

2 篇文章 3 订阅

订阅专栏

算法背景

卡尔曼滤波，包括其非线性改良的变种，比如扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF），都是基于对过程噪声和观测噪声的假设。这些假设包括噪声的统计性质，比如均值、协方差等。

关于卡尔曼滤波两处噪声的来源及影响，在我之前的文章中已经有所提及：

一文详解卡尔曼滤波两处噪声的来源及影响https://blog.csdn.net/weixin_45766278/article/details/128426596

然而在很多实际应用中，过程噪声和观测噪声的统计特性是未知的，甚至可能是随时间变化的。因此固定的过程噪声协方差Q和观测噪声协方差R可能并不适合所有情况。

为了解决这个问题，自适应卡尔曼滤波，也即Adaptive Unscented Kalman Filter (AUKF)被提出。AUKF尝试动态地调整过程噪声协方差Q和观测噪声协方差R，使得滤波器能够适应系统的实际行为。

算法概述及核心运算式

AUKF的主要步骤与标准的UKF类似，包括初始化、Sigma点选择、预测和更新。

只是AUKF在每次更新后，会进行一次调整步骤，通过观察预测误差来动态地更新过程噪声协方差Q和观测噪声协方差R。具体的更新规则可能因具体的应用而不同，但核心思想是，如果预测的协方差比实际的大，那么就需要减小噪声协方差，反之则需要增大。

以下是一个动态调整Q和R的基本公式：

$eq?Q_i%20%3D%20Q_i%20%5Ctimes%20%5Cmax%5Cleft%281%2C%20%5Cfrac%7B%7CP_i%20-%20P_%7Bi_%7B%5Ctext%7Bprev%7D%7D%7D%7C%7D%7BQ_i%7D%5Cright%29%20%5C%5C%20R_i%20%3D%20R_i%20%5Ctimes%20%5Cmax%5Cleft%281%2C%20%5Cfrac%7B%7Cy_i%20-%20%5Chat%7By%7D_i%7C%7D%7BR_i%7D%5Cright%29$

其中：

$eq?%24Q_i%24$ 和 $eq?%24R_i%24$ 是过程噪声协方差矩阵 $eq?%24Q%24$ 和观测噪声协方差矩阵 $eq?%24R%24$ 的元素
$eq?%24P_i%24$ 和 $eq?%24P_%7Bi_%7B%5Ctext%7Bprev%7D%7D%7D%24$ 分别是当前和上一步的状态协方差
$eq?%24y_i%24$ 和 $eq?%24%5Chat%7By%7D_i%24$ 分别是实际和预测的观测值

示例及MATLAB代码

下面通过一个简单的例子来说明其核心思想：

假设我们正在跟踪一个运动目标的位置和速度，其运动是受到一些未知的随机扰动的。我们可以通过雷达来测量这个目标的位置（测量结果会受到随机噪声的影响）。在这个问题中，我们使用AUKF来估计目标的位置和速度。

以下是MATLAB代码：

function [x, P, Q, R] = aukf(f, x, P, h, z, Q, R)
    % 自适应无迹卡尔曼滤波器
    %
    % 输入:
    %   f: 非线性状态方程
    %   x: "先验"状态估计
    %   P: "先验"估计状态协方差
    %   h: 测量方程
    %   z: 当前测量值
    %   Q: 过程噪声协方差 
    %   R: 测量噪声协方差
    %
    % 输出:
    %   x: "后验"状态估计
    %   P: "后验"状态协方差
    %   Q: 调整后的过程噪声协方差
    %   R: 调整后的测量噪声协方差

    n = length(x); % 状态维度

    % Sigma点生成（这里使用简单的对称sigma点选择方法为例）
    X = [x, x*ones(1,n) + chol(P)', x*ones(1,n) - chol(P)'];

    % 时间更新（预测步）
    X = f(X);  % 通过非线性函数f传播sigma点
    x = mean(X, 2);  % 传播后的状态均值
    P_prev = P;  % 保存先前的P用于自适应调整
    P = cov(X') + Q;  % 传播后的状态协方差

    % 测量更新（校正步）
    Y = h(X);  % 通过非线性函数h转换sigma点
    y = mean(Y, 2);  % 转换后的均值
    Pyy = cov(Y') + R;  % 转换后的协方差
    Pxy = cov(X',Y);  % 交叉协方差
    K = Pxy / Pyy;  % 卡尔曼增益

    x = x + K * (z - y);  % 更新后的状态均值
    P = P - K * Pyy * K';  % 更新后的状态协方差

    % 自适应调整
    Q = Q * max(1, abs(diag(P - P_prev)) / diag(Q));
    R = R * max(1, abs(z - h(x)) / diag(R));
end

在这个示例中，f是一个描述目标运动的非线性函数，x是状态向量（包括位置和速度），P是状态协方差矩阵，h是描述雷达测量的非线性函数，z是雷达的测量值，Q是过程噪声协方差矩阵，R是测量噪声协方差矩阵。

需要注意，此代码中的Q和R调整步骤仅使用了简单的规则。在实际应用中，可能需要使用更复杂的规则来调整Q和R。