比较6维UKF，EKF和PF的均方根误差（RMSE）的matlab代码。其中所有函数直接定义

下面是一个比较6维UKF，EKF和PF的均方根误差（RMSE）的matlab代码。其中所有函数直接定义：

% 定义状态转移矩阵
A = [1 1; 0 1];
% 定义测量矩阵
H = [1 0];
% 定义过程噪声协方差矩阵
Q = [0.01 0; 0 0.01];
% 定义观测噪声协方差矩阵
R = 1;
% 定义初始状态
x0 = [0; 0];
% 定义初始状态协方差矩阵
P0 = [1 0; 0 1];

% 定义UKF函数
function [x, P] = UKF(A, Q, H, R, x0, P0, z)
    % 定义状态向量的维度
    n = length(x0);
    % 定义测量向量的维度
    m = length(z);
    % 定义UKF的参数
    alpha = 0.001;
    ki = 0;
    beta = 2;
    lambda = alpha^2*(n+ki)-n;
    c = n+lambda;
    Wm = [lambda/c 0.5/c+zeros(1,2*n)];
    Wc = Wm;
    Wc(1) = Wc(1)+(1-alpha^2+beta);
    c = sqrt(c);
    X = zeros(n,2*n+1);
    X(:,1) = x0;
    P = P0;
    for k = 1:2*n
        X(:,k+1) = X(:,1)+c*sqrt(P)*([zeros(n,1); eye(n)](:,k)-[zeros(n,1); -eye(n)](:,k));
    end
    x = sum(repmat(Wm',n,1).*X,2);
    P = zeros(n);
    for k = 1:2*n+1
        P = P+Wc(k)*(X(:,k)-x)*(X(:,k)-x)';
    end
    P = P+Q;
    % 预测
    X = zeros(n,2*n+1);
    X(:,1) = x;
    for k = 1:2*n
        X(:,k+1) = A*X(:,k);
    end
    x = sum(repmat(Wm',n,1).*X,2);
    P = zeros(n);
    for k = 1:2*n+1
        P = P+Wc(k)*(X(:,k)-x)*(X(:,k)-x)';
    end
    P = P+Q;
    % 更新
    X = zeros(n,m);
    for k = 1:2*n+1
        X(:,k) = H*X(:,k);
    end
    zp = sum(repmat(Wm',m,1).*X,2);
    Pz = zeros(m);
    for k = 1:2*n+1
        Pz = Pz+Wc(k)*(X(:,k)-zp)*(X(:,k)-zp)';
    end
    Pz = Pz+R;
    Pxz = zeros(n,m);
    for k = 1:2*n+1
        Pxz = Pxz+Wc(k)*(X(:,k)-x)*(X(:,k)-x)';
    end
    K = Pxz/Pz;
    x = x+K*(z-zp);
    P = P-K*Pz*K';
end

% 定义EKF函数
function [x,P] = EKF(A, Q, H, R, x0, P0, z)
    % 定义状态向量的维度
    n = length(x0);
    % 预测
    x = A*x0;
    P = A*P0*A'+Q;
    % 更新
    K = P*H'/(H*P*H'+R);
    x = x+K*(z-H*x);
    P = (eye(n)-K*H)*P;
end

% 定义PF函数
function [x, P] = PF(A, Q, H, R, x0, P0, z, N)
    % 定义状态向量的维度
    n = length(x0);
    % 初始化粒子
    X = mvnrnd(x0',P0,N)';
    % 预测
    for i = 1:N
        X(:,i) = A*X(:,i)+mvnrnd([0 0],Q)';
    end
    % 更新
    w = zeros(N,1);
    for i = 1:N
        w(i) = normpdf(z-H*X(:,i),0,R);
    end
    w = w./sum(w);
    % 重采样
    idx = randsample(N,N,true,w);
    X = X(:,idx);
    % 估计
    x = mean(X,2);
    P = cov(X');
end

% 定义主函数
function [rmse_ukf, rmse_ekf, rmse_pf] = compare_rmse()
    % 生成数据
    T = 100;
    x_true = zeros(2,T);
    z = zeros(1,T);
    x_true(:,1) = [0; 0];
    for t = 2:T
        x_true(:,t) = A*x_true(:,t-1)+mvnrnd([0 0],Q)';
        z(t) = H*x_true(:,t)+sqrt(R)*randn;
    end
    % 初始化
    x_ukf = x0;
    P_ukf = P0;
    x_ekf = x0;
    P_ekf = P0;
    x_pf = x0;
    P_pf = P0;
    N = 1000;
    % 运行UKF，EKF和PF
    for t = 2:T
        [x_ukf, P_ukf] = UKF(A, Q, H, R, x_ukf, P_ukf, z(t));
        [x_ekf, P_ekf] = EKF(A, Q, H, R, x_ekf, P_ekf, z(t));
        [x_pf, P_pf] = PF(A, Q, H, R, x_pf, P_pf, z(t), N);
    end
    % 计算RMSE
    rmse_ukf = sqrt(mean((x_true-x_ukf).^2,2));
    rmse_ekf = sqrt(mean((x_true-x_ekf).^2,2));
    rmse_pf = sqrt(mean((x_true-x_pf).^2,2));
end