6维ukf ekf 比较展示matlab代码

以下是一个简单的 MATLAB 代码示例，用于比较基于6维UKF和EKF的状态估计方法：

% 状态转移矩阵
A = [1 0.5 0 0 0 0; 
     0 1 0 0 0 0; 
     0 0 1 0.5 0 0; 
     0 0 0 1 0 0; 
     0 0 0 0 1 0.5; 
     0 0 0 0 0 1];

% 状态转移噪声协方差矩阵
Q = [0.01^2 0 0 0 0 0; 
     0 (0.01/4)^2 0 0 0 0; 
     0 0 0.01^2 0 0 0; 
     0 0 0 (0.01/4)^2 0 0; 
     0 0 0 0 0.01^2 0; 
     0 0 0 0 (0.01/4)^2 0.01^2];

% 测量矩阵
H = [1 0 0 0 0 0; 
     0 0 1 0 0 0; 
     0 0 0 0 1 0];

% 测量噪声协方差矩阵
R = [0.1^2 0 0; 
     0 (0.1/4)^2 0; 
     0 0 0.1^2];

% 初始状态
x0 = [0; 0; 0; 0; 0; 0];

% 初始状态协方差矩阵
P0 = eye(6);

% 生成观测数据
N = 100;
y = simulate_data(A, Q, H, R, x0, N);

% EKF状态估计
[x_hat_ekf, P_ekf] = ekf(A, Q, H, R, y, x0, P0);

% UKF状态估计
[x_hat_ukf, P_ukf] = ukf(A, Q, H, R, y, x0, P0);

% 绘图
plot_states(y, x_hat_ekf, x_hat_ukf);

其中，`simulate_data` 函数用于生成观测数据，`ekf` 和 `ukf` 函数分别实现了EKF和UKF状态估计方法，`plot_states` 函数用于绘制状态估计结果。以下是 `simulate_data` 函数的代码实现：

function y = simulate_data(A, Q, H, R, x0, N)
% 生成观测数据
x = zeros(size(A, 1), N);
y = zeros(size(H, 1), N);
x(:, 1) = x0;
for k = 2:N
    x(:, k) = mvnrnd(A*x(:, k-1), Q)';
    y(:, k) = mvnrnd(H*x(:, k), R)';
end
end

以下是 `ekf` 函数的代码实现：

function [x_hat, P] = ekf(A, Q, H, R, y, x0, P0)
% EKF状态估计
x_hat = zeros(size(A, 1), size(y, 2));
x_hat(:, 1) = x0;
P = P0;
for k = 2:size(y, 2)
    % 预测步骤
    x_hat(:, k) = A*x_hat(:, k-1);
    P = A*P*A' + Q;
    % 更新步骤
    K = P*H'/(H*P*H' + R);
    x_hat(:, k) = x_hat(:, k) + K*(y(:, k) - H*x_hat(:, k));
    P = (eye(size(P)) - K*H)*P;
end
end

以下是 `ukf` 函数的代码实现：

function [x_hat, P] = ukf(A, Q, H, R, y, x0, P0)
% UKF状态估计
x_hat = zeros(size(A, 1), size(y, 2));
x_hat(:, 1) = x0;
P = P0;
for k = 2:size(y, 2)
    % 预测步骤
    [x_sigma, w_sigma] = unscented_transform(x_hat(:, k-1), P, sqrtm(Q));
    [x_pred, P_pred] = calculate_moments(x_sigma, w_sigma, A, []);
    P_pred = P_pred + Q;
    % 更新步骤
    [y_sigma, w_sigma] = unscented_transform(x_pred, P_pred, sqrtm(R));
    [y_pred, Pyy, Pxy] = calculate_moments(y_sigma, w_sigma, [], H);
    K = Pxy/Pyy;
    x_hat(:, k) = x_pred + K*(y(:, k) - y_pred);
    P = P_pred - K*Pyy*K';
end
end

function [x_sigma, w_sigma] = unscented_transform(x, P, W)
% 无损变换
n = length(x);
lambda = n + trace(P)/norm(W)^2 - n;
X = [x, x+sqrt(n+lambda)*W, x-sqrt(n+lambda)*W];
w_sigma = [lambda/(n+lambda), repmat(1/(2*(n+lambda)), 1, 2*n)];
x_sigma = X;
end

function [x, P] = calculate_moments(X, w, A, C)
% 计算均值和协方差矩阵
x = X*w';
n = size(X, 1);
P = zeros(n);
if ~isempty(A)
    for i = 1:2*n+1
        P = P + w(i)*(A*X(:, i) - x)*(A*X(:, i) - x)';
    end
end
if ~isempty(C)
    Pyy = zeros(size(C, 1));
    Pxy = zeros(n, size(C, 1));
    for i = 1:2*n+1
        Pyy = Pyy + w(i)*(C*X(:, i) - C*x)*(C*X(:, i) - C*x)';
        Pxy = Pxy + w(i)*(X(:, i) - x)*(C*X(:, i) - C*x)';
    end
    P = [P, Pxy; Pxy', Pyy];
end
end

以下是 `plot_states` 函数的代码实现：

function plot_states(y, x_hat_ekf, x_hat_ukf)
% 绘制状态估计结果
figure;
subplot(3, 1, 1);
plot(y(1, :), 'b');
hold on;
plot(x_hat_ekf(1, :), 'r--');
plot(x_hat_ukf(1, :), 'g--');
legend('真实状态', 'EKF估计', 'UKF估计');
title('状态1');
subplot(3, 1, 2);
plot(y(2, :), 'b');
hold on;
plot(x_hat_ekf(3, :), 'r--');
plot(x_hat_ukf(3, :), 'g--');
legend('真实状态', 'EKF估计', 'UKF估计');
title('状态2');
subplot(3, 1, 3);
plot(y(3, :), 'b');
hold on;
plot(x_hat_ekf(5, :), 'r--');
plot(x_hat_ukf(5, :), 'g--');
legend('真实状态', 'EKF估计', 'UKF估计');
title('状态3');
end

该代码将生成一个包含三个子图的图形，每个子图显示观测数据、EKF估计结果和UKF估计结果的比较。