【滤波跟踪】基于扩展卡尔曼滤波器、无迹卡尔曼滤波器、粒子滤波器和自适应 H-Infinity 滤波器实现各向同性和各向异性干扰模式下无人机导航附MATLAB代码

% 导航系统模型
% 状态方程：x(k) = A * x(k-1) + B * u(k) + w(k)
% 观测方程：y(k) = C * x(k) + v(k)

% 初始化导航系统参数
A = eye(4); % 状态转移矩阵
B = eye(4); % 控制输入矩阵
C = eye(2); % 观测矩阵
Q = eye(4); % 状态噪声协方差矩阵
R = eye(2); % 观测噪声协方差矩阵

% 初始化滤波器参数
x0 = [0; 0; 0; 0]; % 初始状态估计
P0 = eye(4); % 初始状态协方差矩阵

% 生成模拟数据
T = 100; % 时间步数
u = randn(4, T); % 控制输入
x_true = zeros(4, T); % 真实状态
x_true(:, 1) = x0;
y = zeros(2, T); % 观测数据
for k = 2:T
x_true(:, k) = A * x_true(:, k-1) + B * u(:, k) + sqrt(Q) * randn(4, 1);
y(:, k) = C * x_true(:, k) + sqrt® * randn(2, 1);
end

% 扩展卡尔曼滤波器
% 初始化滤波器参数
xEKF = x0;
PEKF = P0;
xEKF_est = zeros(4, T); % 估计的状态
xEKF_est(:, 1) = xEKF;

for k = 2:T
% 预测步骤
xEKF_pred = A * xEKF + B * u