matlab-基于扩展卡尔曼滤波EKF的轨迹跟踪matlab仿真-源码

clc; clear; close all; warning off; addpath(genpath(pwd)); %**************************************************************************** %更多关于matlab和fpga的搜索“fpga和matlab”的CSDN博客: %matlab/FPGA项目开发合作 %https://blog.csdn.net/ccsss22?type=blog %**************************************************************************** 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.1 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=? Q=diag([0.1 0.1 degreeToRadian(1)]).^2; % Covariance Matrix for observation % convR=? R=diag([1.5 1.5 degreeToRadian(3)]).^2; % Other Intial % ? % 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(xPred, u); PPred= F*PxEkf*F' + Q; % Update % xEkf=? % 已修改 应该没问题 H=jacobH(xPred); y = z - doObservation(xPred); S = H*PPred*H' + R; K = PPred*H'*inv(S); xEkf = xPred + K*y; PxEkf = (eye(size(xEkf,1)) - K*H)*PPred; % 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(i,removeStep)==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);