EKF机器人定位-MATLAB.m

使用EKF算法（拓展卡尔曼滤波）来估计机器人的位置信息，并实现可视化展示。该EKF算法还与里程计模型和GPS模型估计进行对比，来判断其估计效果。（运行时记得把.m文件改成英文，否则无法运行）

function [] = ekf_localization（）
    close all;
    clear all;

    disp（‘EKF Start!‘）

    time = 0;
    global endTime; % [sec]
    endTime = 60;
    global dt;
    dt = 0.1; % [sec]

    removeStep = 5;

    nSteps = ceil（（endTime - time）/dt）;

    estimation.time=[];
    estimation.u=[];
    estimation.GPS=[];
    estimation.xOdom=[];
    estimation.xEkf=[];
    estimation.xTruth=[];

    % State Vector [x y yaw]‘
    xEkf=[0 0 0]‘;
    PxEkf = eye（3）;

    % Ground True State
    xTruth=xEkf;

    % Odometry Only
    xOdom=xTruth;

    % Observation vector [x y yaw]‘
    z=[0 0 0]‘;

    % Simulation parameter
    global noiseQ
    noiseQ = diag（[0.1 0 degreeToRadian（10）]）.^2; %[Vx Vy yawrate]

    global noiseR
    noiseR = diag（[0.5 0.5 degreeToRadian（5）]）.^2;%[x y yaw]
    
    % Covariance Matrix for motion
    convQ=eye（3）;
    
    % Covariance Matrix for observation
    convR=noiseR;

    % Other Intial
    xPred = [0 0 0]‘;
    F = zeros（3）;
    H = zeros（3）;
    
    % Main loop
    for i=1 : nSteps
        time = time + dt;
        % Input
        u=robotControl（time）;
        % Observation
        [zxTruthxOdomu]=prepare（xTruth xOdom u）;

        % ------ Kalman Filter --------
        % Predict
        xPred =  doMotion（xEkf u）;
        F = jacobF（xEkf u）;
        convQ = F*convQ*F‘+ noiseQ;     
        
        % Update
        H = jacobH（xPred）;
        PxEkf = convQ*H‘*inv（H*convQ*H‘+convR）;
        xEkf= doObservation（z xPredPxEkf）;
        convQ=（eye（3）-PxEkf*H）*convQ; 

        % -----------------------------
        % Simulation estimation
        estimation.time=[estimation.time; time];
        estimation.xTruth=[estimation.xTruth; xTruth‘];
        estimation.xOdom=[estimation.xOdom; xOdom‘];
        estimation.xEkf=[estimation.xEkf;xEkf‘];
        estimation.GPS=[estimation.GPS; z‘];
        estimation.u=[estimation.u; u‘];

        % Plot in real time
        % Animation （remove some flames）
        if rem（iremoveStep）==0
            %hold off;
            plot（estimation.GPS（:1）estimation.GPS（:2）‘*m‘ ‘MarkerSize‘ 5）;hold on;
            plot（estimation.xOdom（:1）estimation.xOdom（:2）‘.k‘ ‘MarkerSize‘ 10）;hold on;
            plot（estimation.xEkf（:1）estimation.xEkf（:2）‘.r‘‘MarkerSize‘ 10）;hold on;
            plot（estimation.xTruth（:1）estimation.xTruth（:2）‘.b‘ ‘MarkerSize‘ 10）;hold on;
            axis equal;
            grid on;
            drawnow;
            %movcount=movcount+1;
            %mov（movcount） = getframe（gcf）;% アニメーションのフレームをゲットする
        end 
    end
    close    
    finalPlot（estimation）;
end

% control
function u = robotControl（time）
    global endTime;
    T = 10; % sec
    Vx = 1.0; % m/s
    Vy = 0.2; % m/s
    yawrate = 5; % deg/s
    
    % half
    if time > （endTime/2）
        yawrate = -5;
    end
    
    u =[ Vx*（1-exp（-time/T）） Vy*（1-exp（-time