EKF-SLAM matlab仿真(2)_水下slam仿真系统

%-------------------------------------------------------------------------
% FILE: slam.m 
% DESC: Implements the SLAM algorithm based on a robot model and sensor 
%       data from Nebot.
%landmarks(真实路标) 
%estbeac=estimation(估计)+beacon(路标)，将estbeac的值赋给landmarks然后将estbeac清除。
%xest                                   状态矩阵
%Pest                                   协方差矩阵
%A W Jh                                 雅可比矩阵
%Q                                      过程激励噪声协方差矩阵
%k                                      迭代次数
%K                                      卡尔曼增益
%numStates                              状态的数量  初始化时候就三个，即位置(x，y)和姿态(phi)
%phi                                    姿态，即与x轴的夹角
%Steering                               转向角
%alpha                                  转向角
%Ve = Velocity                          后轮的转速
%Vc                                     车水平平移速度
%zlaser=[rangeArray; bearingArray];     机器人与路标的距离和方位
%--------------------------------------------------------------------------
clear all; clc;
load('data_set');
load('beac_juan3.mat');   %beac_juan3.mat中内容为路标,命令whos –file查看该文件中的内容
landmarks=estbeac; clear estbeac; % true landmark positions (measured w/GPS sensor)
%---------------------  DATA INITIALIZATION   -----------------------------
% Vehicle constants
L=2.83;     % distance between front and rear axles(前后轮轴之间的距离)
h=0.76;     % distance between center of rear axle and encoder(后轮轴中心与编码器的距离)
b=0.5;      % vertical distance from rear axle to laser(后轮轴中心到激光传感器的垂直距离)
a=3.78;     % horizontal distance fro