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

以下是一个简单的 MATLAB 代码示例，用于比较6维UKF、EKF和PF的均方根误差（RMSE）。

%% 定义函数

% 6维UKF
function [x,P] = ukf(fstate,x,P,hmeas,z,Q,R)
    % 定义sigma点参数
    L=numel(x); % 状态维度
    alpha=1e-3; % 默认值
    ki=0; % 默认值
    beta=2; % 默认值
    lambda=alpha^2*(L+ki)-L;
    c=L+lambda;
    Wm=[lambda/c 0.5/c+zeros(1,2*L)];
    Wc=Wm;
    Wc(1)=Wc(1)+(1-alpha^2+beta);
    c=sqrt(c);
    
    % 生成sigma点
    X=sigmas(x,P,c);
    
    % 预测
    [x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);
    
    % 测量更新
    [z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,size(z,1),R);
    P12=X2*diag(Wc)*Z2';
    K=P12*inv(P2);
    x=x1+K*(z-z1);
    P=P1-K*P12';
end

% 6维EKF
function [x,P]= ekf(fstate,x,P,hmeas,z,Q,R)
    % 预测
    [x1,A]=jaccsd(fstate,x); % 线性化
    P1=A*P*A'+Q;
    
    % 测量更新
    [z1,H]=jaccsd(hmeas,x1); % 线性化
    P12=P1*H';
    K=P12*inv(H*P1*H'+R);
    x=x1+K*(z-z1);
    P=P1-K*P12';
end

% 6维PF
function [x,particles]=pf(x,particles,z,Q,R)
    % 预测
    particles=motion_model(particles,Q);
    
    % 权重更新
    weights=measurement_model(z,particles,R);
    weights=weights./sum(weights);
    
    % 重采样
    idx=resample(weights);
    particles=particles(:,idx);
    
    % 计算估计值
    x=mean(particles,2);
end

% 生成sigma点
function X = sigmas(x,P,c)
    A = c*chol(P)';
    Y = x(:,ones(1,numel(x)));
    X = [x Y+A Y-A];
end

% 无约束下的UT
function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
    L=size(X,2);
    y=zeros(n,1);
    Y=zeros(n,L);
    for k=1:L
        Y(:,k)=f(X(:,k));
        y=y+Wm(k)*Y(:,k);
    end
    Y1=Y-y(:,ones(1,L));
    P=Y1*diag(Wc)*Y1'+R;
end

% 非线性函数的Jacobian矩阵
function [z,A]=jaccsd(fun,x)
    z=fun(x);
    n=numel(x);
    m=numel(z);
    A=zeros(m,n);
    h=n*eps;
    for k=1:n
        x1=x;
        x1(k)=x1(k)+h*i;
        A(:,k)=imag(fun(x1))/h;
    end
end

% 状态转移模型
function particles=motion_model(particles,Q)
    [n,m]=size(particles);
    particles=particles+sqrt(Q)*randn(n,m);
end

% 测量模型
function weights=measurement_model(z,particles,R)
    [n,m]=size(particles);
    v=repmat(z,1,m)-particles;
    likelihood=exp(-0.5*sum(v.*(inv(R)*v),1));
    weights=likelihood./sum(likelihood);
end

% 重采样
function idx=resample(weights)
    N=numel(weights);
    Q=cumsum(weights);
    T=zeros(1,N);
    T(1)=rand/N;
    for i=2:N
        T(i)=T(i-1)+rand/N;
    end
    [~,idx] = histc(T,[0;Q]);
end

%% 主程序

% 设定参数
dt=0.1; % 采样时间
T=10; % 仿真时间
R=1; % 测量噪声协方差
Q=diag([0.1 0.1 0.1 0.1 0.1 0.1]); % 状态转移噪声协方差

% 初始化
x0 = [0; 0; 0; 0; 0; 0]; % 初始状态
P0 = eye(6); % 初始协方差
xhat_ukf = x0; % UKF估计
xhat_ekf = x0; % EKF估计
xhat_pf = x0; % PF估计
P_ukf = P0; % UKF协方差矩阵
P_ekf = P0; % EKF协方差矩阵
particles_pf = repmat(x0,1,1000); % PF粒子

% 仿真
for t=0:dt:T
    % 状态转移
    x = [cos(t); sin(t); t; 0.1*cos(t); 0.1*sin(t); 0.1];
    
    % 测量
    z = [sin(0.5*t); cos(t)];
    
    % UKF估计
    [xhat_ukf,P_ukf] = ukf(@fstate,xhat_ukf,P_ukf,@hmeas,z,Q,R);
    
    % EKF估计
    [xhat_ekf,P_ekf] = ekf(@fstate,xhat_ekf,P_ekf,@hmeas,z,Q,R);
    
    % PF估计
    [xhat_pf,particles_pf] = pf(xhat_pf,particles_pf,z,Q,R);
    
    % 记录估计误差
    err_ukf = x - xhat_ukf;
    err_ekf = x - xhat_ekf;
    err_pf = x - xhat_pf;
    RMSE_ukf = sqrt(sum(err_ukf.^2)/6);
    RMSE_ekf = sqrt(sum(err_ekf.^2)/6);
    RMSE_pf = sqrt(sum(err_pf.^2)/6);
    
    % 输出结果
    disp(['Time: ' num2str(t) ' UKF RMSE: ' num2str(RMSE_ukf) ' EKF RMSE: ' num2str(RMSE_ekf) ' PF RMSE: ' num2str(RMSE_pf)]);
end

%% 定义状态转移函数和测量函数

% 状态转移函数
function xdot=fstate(x)
    xdot = [x(4)*cos(x(3)); 
            x(4)*sin(x(3));
            x(5);
            0;
            0;
            0];
end

% 测量函数
function z=hmeas(x)
    z = [sin(0.5*x(3));
         cos(x(3))];
end

在上述代码中，我们定义了三个函数：`ukf`、`ekf`和`pf`，分别实现了6维UKF、EKF和PF。这些函数的输入包括状态转移函数、测量函数、初始状态和协方差矩阵、状态转移噪声和测量噪声的协方差矩阵等参数。我们还定义了一些辅助函数，例如生成sigma点、UT算法、非线性函数的Jacobian矩阵、状态转移模型、测量模型和重采样等。

在主程序中，我们首先设定了仿真参数，并初始化了UKF、EKF和PF的估计值和协方差矩阵，以及PF粒子。然后，我们在一个循环中模拟了状态转移和测量，并分别使用UKF、EKF和PF进行估计和记录误差。最后，我们输出了三种方法的RMSE值。

需要注意的是，这个示例代码可能并不是最优的实现方式，而且需要根据具体问题进行修改和调整。