SLAM-MATLAB-code
所属分类:matlab编程
开发工具:matlab
文件大小:14364KB
下载次数:375
上传日期:2013-07-26 14:03:51
上 传 者:
lzh0115
说明: slam仿真工具箱,包含slam的matlab仿真源程序以及slam学习程序
(slam simulation toolbox, matlab simulation source code contains slam and slam learning process)
文件列表:
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\12154.fig (2460125, 2008-04-17)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\add_control_noise.m (277, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\add_observation_noise.m (343, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\augment.m (1101, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\augment_associate_known.m (490, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\compute_steering.m (1140, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\configfile.m (2046, 2004-03-26)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\data_associate.m (1557, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\data_associate_known.m (879, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\ekfslam_sim.m (6670, 2004-03-26)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\example_webmap.mat (1072, 2004-03-26)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\frontend.fig (54416, 2004-02-16)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\frontend.m (5629, 2004-03-26)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\get_observations.m (1378, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\hs_err_pid3100.log (17515, 2011-03-06)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\KF_cholesky_update.m (611, 2004-03-08)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\KF_IEKF_update.m (1470, 2004-03-26)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\KF_simple_update.m (632, 2004-02-13)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\line_plot_conversion.m (827, 2004-02-13)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\observe_heading.m (417, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\observe_model.m (838, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\pi_to_pi.m (239, 2004-02-13)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\plot_feature_loci.m (586, 2004-03-26)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\predict.m (852, 2004-03-16)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\song.mat (216, 2008-04-17)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\song2.mat (817, 2008-04-17)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\TransformToGlobal.m (433, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\update.m (1125, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\update_iekf.m (1045, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\vehicle_model.m (340, 2004-03-25)
SLAM MATLAB 代码--8套\EKF-SLAM Simulator_v1.02\结果1.fig (199977, 2008-04-17)
SLAM MATLAB 代码--8套\ekfslam_v1.0\add_control_noise.m (277, 2004-03-25)
SLAM MATLAB 代码--8套\ekfslam_v1.0\add_observation_noise.m (343, 2004-03-25)
SLAM MATLAB 代码--8套\ekfslam_v1.0\augment.asv (1119, 2010-06-08)
SLAM MATLAB 代码--8套\ekfslam_v1.0\augment.m (1247, 2010-06-08)
SLAM MATLAB 代码--8套\ekfslam_v1.0\augment_associate_known.m (490, 2004-03-25)
SLAM MATLAB 代码--8套\ekfslam_v1.0\compute_steering.m (1217, 2010-06-07)
SLAM MATLAB 代码--8套\ekfslam_v1.0\configfile.m (2046, 2010-06-09)
SLAM MATLAB 代码--8套\ekfslam_v1.0\data_associate.m (1557, 2004-03-25)
... ...
Extended Kalman Filter Navigation System-Slam 2D Problem
Documentation
The following Matlab code implements the extended Kalman filter to be use in a Slam
application with encoders and laser sensors.
The data set used was taking in the Car Park, with artificial beacons. This will let us
to make a comparison between the real and estimated position of the beacons and
analyse the filter’s performance.
To run the code:
Run the file “slam”. This sets up a graphical window that will let you to see the path
estimation and the difference with the “real” path (Gps).
To start the filter, run the script file slam.
Function Definitions:
slam:
Function to run the code
FindT:
Function to find the index of the time sensor vector, for the
selected time.
[GPSTIME,LONG,LAT]=ReadGpsData(file):
Function to read the gps data, and transform to a local navigation
frame with respect to a reference point.
[Time,STEERING,SPEED1]=ReadUteData(file):
Similar to the ReadGpsData function, is to read the encoder data,
from a mat file. ( Not used in this example since all sensor data is
in a single matlab file.
Pred(dt,u):
Function to implement the prediction stage of the EKF. The inputs are
the estimated state vector, the estimates covariance matrix and the
encoder data (velocity and steering).
[beacon]=getpos:
Function to get the position of the artificial beacons, from a gps
data set.
[index,innov,S]=asoc_update(zlaser,pointer,updatee):
Function to make the association of the observation with one beacon,
and eventually the update. In the case one beacon was already
associated to the observation, or if it is the first time we are
seeing it, then we have to incorporate this to the vector state and
make an update with this new state.
The flag updatee is used to know if it is an update or if it is only
an association. A flag index is return to know the result of the chi
square test.
new_state(zlaser):
This function is used when we have a new observation that was not
associated with any beacon of the state vector. The Cartesian
position of the “new” beacon is evaluated and insert as a new state.
A large number for the variance uncertainty is set. This may
introduce numerical problems if not properly selected.
[index2]=Zone_Probe(zlaser):
The association task is very expensive. It is important to try
association only with the beacons that are in the area. This area can
be evaluated with the uncertainty of the vehicle. It returns the
indices to all the beacons that are within a certain area.
laserview(RR,a,xp,hhh3,hhh4,hhh2,LASERr,LASERo):
Function to plots the laser scan. Plots the standard reflections in
one colour and the high intensity reflection in a different colour.
[LASERr,LASERo,RR,a]=getdata(laser):
Function to get the range and bearing to the beacons. Gets the high
intensity point and evaluate the centre of the beacon.
[innov, S]=update_gps(zgps):
Implement the update stage of the EKF with a gps observation. The
inputs are the predicted state vector, the predicted covariance
matrix, and the gps data (longitude and latitude). The GPS is only
used for initialisation purposes.
[meanq,q,chib_up,chib_low,timeinn]=inn_analyse(inn,S):
Function to analyse innovation sequences. Is used only to plot the
Normalised Innovations and Innovation Confidence Bounds.
Rxx=auto(x):
Computes autocorrelation of input data set. N is the number of data
points, x is a column matrix holding the input data set.
Uses fft method as advertised in Maybeck p193.
Plots:
Script file to run all the plots when the filter finish.
SaveStates(states,diagcov,times,Flag):
Function to save the states and the covariance matrix in arrays. All
the beacons in the state vector are saved. Another vector is saved
called FlagStates with the number of times the beacon has been seen..
近期下载者:
相关文件:
收藏者: