【自动驾驶汽车量子群粒子过滤器】用于无人驾驶汽车列车定位的量子粒子滤波研究（Matlab代码实现）

figure(2);
    set(gca,'FontSize',12);
    clf;
    hold on
    plot(X(1, k), X(2, k), 'r.', 'markersize',50);  % System status
    axis([0 100 0 100]);
    plot(P(1, :), P(2, :), 'k.', 'markersize',5);   % Particle position
    plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); % Center
    legend('True State', 'Particle', 'Center of Particles');
    xlabel('x', 'FontSize', 20); ylabel('y', 'FontSize', 20);
    title('Real Gaussian Errors');
    grid;
    hold off
    pause(0.5);
end
%% 1.5 dimension quantum space
P = P_init;    % Particle starts off 
% Jumps now
for k = 2 : T   
    % Prediction
    for i = 1 : N
        P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10*log10(Q));
    end
    % Find the center，direction of moving
    center = sum(P, 2) / N;      % Center of particle 
    path = Z(:, k) - Z(:, k-1);  % Vector of path
    pathAngle = atan2(path(2, 1), path(1, 1));  % Direction of moving
    % Initialization of extra parameters 
    PLeft = zeros(2, N);        % Left party members
    PRight = zeros(2, N);       % Right party members
    WLeft = zeros(N, 1);        % Weight of left 
    WRight = zeros(N, 1);       % Weight of right
    ILeft = 1;
    IRight = 1;
    % Particles are divided into either of left spin party or right spin party 
    for i = 1 : N
        % Space transformation, driver's moving view, counter-clock wise，0~2pi
        path = P(:, i) - center;
        partiAngle = atan2(path(2, 1), path(1, 1));     % Angle towards the particle center
        wAngle = mod(partiAngle - pathAngle + 2*pi, 2*pi);  
        % 0~pi belongs to left  pi~2*pi belongs to right
        if wAngle > 0 && wAngle <= pi
            PLeft(:, ILeft) = P(:, i);
            dist = norm(PLeft(:, ILeft)-Z(:, k));     % Left distance to the observer
            WLeft(ILeft) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^1.5 / 1.5 / R);   % Left quantum weight
            ILeft = ILeft + 1;
        else
            PRight(:, IRight) = P(:, i); 
            dist = norm(PRight(:, IRight)-Z(:, k));     % Right distance to the observer
            WRight(IRight) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^1.5 / 1.5 / R);  % Right quantum weight
            IRight = IRight + 1;
        end
    end
    CLeft = ILeft - 1;
    CRight = IRight - 1;
    % Left spin normalization
    wsum = sum(WLeft);
    for i = 1 : CLeft
        WLeft(i) = WLeft(i) / wsum;
    end   
    % Left resampling 
    for i = 1 : CLeft
        wmax = 2 * max(WLeft) * rand;  % Use the same rule as baseline
        index = randi(CLeft, 1);