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 filters 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..
没有合适的资源?快使用搜索试试~ 我知道了~
13 SLAM算法程序,来自悉尼大学的作品,主要功能是实现SLAM算法+仿真结果和运行方法.zip
共39个文件
gif:12个
ps:9个
jpg:3个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
0 下载量 60 浏览量
2023-06-02
19:58:00
上传
评论 1
收藏 526KB ZIP 举报
温馨提示
1.版本:matlab2014/2019a/2021a,内含运行结果,不会运行可私信 2.领域:智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真,更多内容可点击博主头像 3.内容:标题所示,对于介绍可点击主页搜索博客 4.适合人群:本科,硕士等教研学习使用 5.博客介绍:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可si信
资源推荐
资源详情
资源评论
收起资源包目录
13 SLAM算法程序,来自悉尼大学的作品,主要功能是实现SLAM算法+仿真结果和运行方法.zip (39个子文件)
13 SLAM算法程序,来自悉尼大学的作品,主要功能是实现SLAM算法+仿真结果和运行方法
slam
car_data.m 4KB
slam_turn_exp.ps 11KB
slam_path.ps 34KB
slam_autocorr.ps 27KB
slam_innov.ps 25KB
slam.m 29KB
slam_cov.ps 86KB
slam_beac_cov.ps 448KB
beac_juan3.mat 432B
~$cumentation.doc 162B
documentation.doc 35KB
modelling_info
data_file.htm 4KB
data_file_files
Thumbs.db 7KB
image001.jpg 26KB
Ute_modeling_info.htm 11KB
Ute_modeling_info_files
image009.gif 1KB
Thumbs.db 17KB
image007.gif 2KB
image005.gif 423B
image014.gif 53KB
image013.gif 4KB
image002.gif 148B
image012.gif 4KB
image003.gif 654B
image001.gif 686B
image004.gif 92B
image011.gif 2KB
image008.jpg 8KB
image010.jpg 9KB
image006.gif 814B
documentation.pdf 14KB
data_set.mat 2.22MB
slam_res.ps 42KB
slam.ps 135KB
slam_turn.ps 17KB
readme.txt 4KB
说明.txt 367B
仿真咨询.png 2KB
更多代码关注我.png 446B
共 39 条
- 1
资源评论
Matlab科研辅导帮
- 粉丝: 1w+
- 资源: 7491
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功