基于新息协方差匹配的自适应UKF (CM-AUKF) vs 经典UKF对比，以二维雷达跟踪与滤波为背景
原创代码，禁止翻卖

程序简介

代码功能与方法概述

本代码针对二维平面目标跟踪问题，构建了一个基于雷达测距–测角观测的非线性状态估计模型，对比研究了基于新息协方差匹配（Covariance Matching）的自适应无迹卡尔曼滤波（CM-AUKF）与经典UKF在噪声统计信息失配条件下的性能差异。目标运动采用匀速模型，观测为极坐标形式，具有典型的非线性特征。

系统状态定义为
$$\mathbf{x}\ast k=[x_k,v\ast x,k,y_k,v_{y,k}{]}^{\mathrm{T}},$$
其离散时间状态转移模型为
$${\mathbf{x}}_{k+1}=\mathbf{F}{\mathbf{x}}_k+\mathbf{G}{\mathbf{w}}_k,$$
其中 (\mathbf{w}_k) 为零均值高斯过程噪声，(\mathbf{Q}=\mathbb{E}[\mathbf{w}_k\mathbf{w}_k^{\mathrm T}])。

雷达观测模型为非线性极坐标形式：
$${\mathbf{z}}_k=\left[\begin{array}{c}{r}_k{\theta }_k\end{array}\right]$$

$$\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\arctan (y_k/x_k)\end{array}\right]\ast {\mathbf{v}}_k,$$
其中 (\mathbf{v}_k) 为观测噪声，其协方差矩阵为 (\mathbf{R})。

在滤波实现上，UKF通过无迹变换对状态与观测的非线性传播进行近似，利用Sigma点计算预测均值和协方差，并由新息
$${\mathbf{\nu }}_k={\mathbf{z}}_k-{\hat{\mathbf{z}}}_k$$
完成状态更新。

CM-AUKF在此基础上引入新息协方差匹配机制：在滑动窗口内估计实际新息协方差
$$\mathbf{C}\ast k=\frac{1}{M}\sum \ast {i=k-M+1}^k{\mathbf{\nu }}_i{\mathbf{\nu }}_i^{\mathrm{T}},$$
并将其与UKF理论新息协方差${\mathbf{S}}_k$进行对比，根据二者偏差自适应调整观测噪声协方差$\mathbf{R}$，同时结合迹比准则对过程噪声协方差$\mathbf{Q}$进行修正。相比之下，经典UKF采用固定的、事先设定的$\mathbf{Q})和(\mathbf{R}$，不具备在线自适应能力。

仿真结果从轨迹、位置与速度误差、新息统计特性以及 (\mathbf{Q})、(\mathbf{R}) 的估计收敛过程等多个角度，对比验证了CM-AUKF在噪声统计不确定条件下的稳健性与精度优势，尤其在滤波初期和模型失配场景中表现更为明显。

运行结果

目标轨迹（在代码中可直接修改）：

估计的位置误差：

估计的速度误差：

自适应曲线：

命令行输出的结果：

MATLAB源代码

部分代码如下：

% 基于新息协方差匹配的自适应UKF (CM-AUKF) vs 经典UKF对比
% 状态: 二维平面匀速运动 [x, vx, y, vy],观测: 雷达测角与测距 [r, theta]
% 作者：matlabfilter
% 2025-10-07/Ver1

clear; clc; close all;
rng(0);

%% 仿真参数设置
dt = 0.1;           % 采样时间
N = 200;            % 仿真步数
M = 15;             % 滑动窗口长度

% 真实过程噪声和观测噪声
q_true = 0.1;       % 过程噪声标准差
sigma_r = 5;        % 距离测量噪声标准差
sigma_theta = 0.02; % 角度测量噪声标准差 (rad)

%% UKF参数设置
n = 4;              % 状态维度
m = 2;              % 观测维度
alpha = 1e-3;       % UT变换参数：sigma点分布范围 (通常取0.001~1)
beta = 2;           % UT变换参数：先验分布信息 (高斯分布取2)
kappa = 0;          % UT变换参数：次要缩放参数 (通常取0或3-n)

lambda = alpha^2 * (n + kappa) - n;
Wm = zeros(2*n+1, 1);   % 均值权重
Wc = zeros(2*n+1, 1);   % 协方差权重

Wm(1) = lambda / (n + lambda);
Wc(1) = lambda / (n + lambda) + (1 - alpha^2 + beta);
for i = 2:(2*n+1)
    Wm(i) = 1 / (2 * (n + lambda));
    Wc(i) = 1 / (2 * (n + lambda));
end

%% 状态转移矩阵 (匀速运动模型)
F = [1, dt, 0,  0;
     0, 1,  0,  0;
     0, 0,  1,  dt;
     0, 0,  0,  1];

% 过程噪声协方差矩阵 Q
G = [dt^2/2, 0;
     dt,     0;
     0,      dt^2/2;
     0,      dt];
Q_true = q_true^2 * (G * G');

% 观测噪声协方差矩阵 R
R_true = diag([sigma_r^2, sigma_theta^2]);

%% 初始化
% 真实初始状态 [x, vx, y, vy]
x_true = [1000; 50; 1000; -30];

% CM-AUKF初始估计
x_aukf = x_true + [50; 10; 50; 10]; % 添加初始误差
P_aukf = diag([100^2, 20^2, 100^2, 20^2]); % 初始协方差

% 经典UKF初始估计
x_ukf = x_aukf;  % 使用相同初始值
P_ukf = P_aukf;

% CM-AUKF: 自适应估计初始化 (故意设置不准确)
Q_aukf = Q_true * 2;  % Q的初始估计
R_aukf = R_true * 5;  % R的初始估计

% 经典UKF: 使用不准确的固定Q和R (与AUKF初始值相同)
Q_ukf = Q_aukf;   % 固定不变
R_ukf = R_aukf;   % 固定不变

%% 定义变量
x_true_hist = zeros(4, N);
x_aukf_hist = zeros(4, N);
x_ukf_hist = zeros(4, N);
z_hist = zeros(2, N);

% CM-AUKF变量
innovation_aukf_hist = zeros(2, N);
C_k_hist = zeros(2, 2, N);  % 实际新息协方差
S_k_aukf_hist = zeros(2, 2, N);  % CM-AUKF理论新息协方差
Q_aukf_trace_hist = zeros(1, N);
R_aukf_hist = zeros(2, 2, N);

% 经典UKF变量
innovation_ukf_hist = zeros(2, N);
S_k_ukf_hist = zeros(2, 2, N);  % 经典UKF理论新息协方差

% CM-AUKF滑动窗口存储
innovation_window = zeros(2, M);
window_idx = 1;

%% 定义非线性函数
% 状态转移函数 (线性，但保持函数形式以适配UKF框架)
state_transition = @(x) F * x;

% 观测函数 (非线性：极坐标观测)
observation_function = @(x) [sqrt(x(1)^2 + x(3)^2); atan2(x(3), x(1))];

%% 主循环

完整代码下载链接：https://download.csdn.net/download/callmeup/92122513

或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者