基于EKF（拓展卡尔曼滤波器）的机器人定位（MATLAB编程）

最新推荐文章于 2024-08-30 17:36:51 发布

wxxj_yz000

最新推荐文章于 2024-08-30 17:36:51 发布

阅读量4.2k

点赞数 9

文章标签：机器学习线性代数动态规划算法概率论

本文链接：https://blog.csdn.net/wffs_yz000/article/details/104069086

版权

本文介绍了基于EKF（拓展卡尔曼滤波器）的机器人定位方法，阐述了EKF的基本原理，并展示了MATLAB编程实现后的程序结果。通过对比EKF模型和里程计模型（Odometry）的估计误差，发现EKF模型误差稳定，而Odometry误差随时间增加，表明EKF在机器人定位中有优势。

摘要由CSDN通过智能技术生成

1 EKF原理

EKF是在KF的基础上，增加了对线性要求的拓展，即可以采用非线性函数表示运动方程和观测方程。
EKF的基本思想如下：
在这里插入图片描述

2 程序结果

Odometry表示里程计模型。
在这里插入图片描述

3 程序代码

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
        [z,xTruth,xOdom,u]=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, xPred,PxEkf);
        convQ=(eye(3)-PxEkf*H)*convQ;