机器人学，计算机视觉 MATLAB 心得
前言：看这本书目的是让自己熟悉matlab做图，只把matlab当做作图的软件，并且能够熟悉位姿变换的那些知识在matlab当中是如何使用的，多一些理论基础， 我自己的时间规划是1周的时间吧概率机器人和这本书看完，并且将hectorslam的源码能够分析清楚，周二的时候，我要进行测试实验，能够找到影响小车建图性能的原因。
建议用2011版本以后的matlab.
版本太低的话，很多东西不是很兼容
出现的问题
导入matlab工具箱
参考博客：
http://blog.csdn.net/zyf1109/article/details/53423385
由于版本问题导致的现象
换了matlab的版本之后
切换到这个目录下面：
表示将（1,2）
trplot2(T1,'frame','1','color','b')
这个函数trplot只是能在这个工具箱当中使用
Rx表示绕x轴旋转a角度之后的正交旋转矩阵：
同理，我们扩展一下：
分别对应：
R = rotx(pi/2)
R = roty(pi/2)
R = rotz(pi/2)
扩展：使用tranimate(R)可以画出动画的效果
正交矩阵有9个元素，但是他们并不是独立的，因为需要正交的单位阵啊，每一列都是单位长度，这提供了3个约束，列与列之间相互正交，提供另外的3个约束，这样9个元素加上6个约束，实际上就只有3个独立的值。小白问：啥叫约束啊，约束其实就是限制条件呗。()
x-y-z欧拉角
还记得我们一另一篇博客里面提到的rpy么？roll翻滚 pitch俯仰 yaw航偏
假设roll=0.1 pitch=0.2 yaw=0.3，那么如何通过欧拉角来求旋转矩阵呢？
>> R=rpy2r(0.1,0.2,0.3)
R =
0.9363 -0.2896 0.1987
0.3130 0.9447 -0.0978
-0.1593 0.1538 0.9752同样，如果你已经知道了旋转矩阵，那么该如何求出欧拉角呢？
>> gamma=tr2rpy(R)
gamma =
0.1000 0.2000 0.3000也就是说，r p y 来描述位姿的时候，会产生奇异点
那么旋转矩阵的特征值和特征向量怎么求呢？
矩阵lambda中的对角线的元素就是返回的特征值，其对应的特征向量就是v当中的列向量
>> [v,lambda]=eig(R)
v =
0.6655 + 0.0000i 0.6655 + 0.0000i 0.3379 + 0.0000i
-0.1220 - 0.6079i -0.1220 + 0.6079i 0.4807 + 0.0000i
-0.2054 + 0.3612i -0.2054 - 0.3612i 0.8092 + 0.0000i
lambda =
0.9281 + 0.3724i 0.0000 + 0.0000i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.9281 - 0.3724i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i举例:
特征值：0.9281 + 0.3724i
特征向量：
0.6655 + 0.0000i
-0.1220 - 0.6079i
-0.2054 + 0.3612i四元数
四元数是复数的扩展，或者叫超复数 q=w+xi+yj+zk
正交的复数：
R=rpy2r(0.1,0.2,0.3)
R =
0.9363 -0.2896 0.1987
0.3130 0.9447 -0.0978
-0.1593 0.1538 0.9752
>> q=Quaternion(R)
q =
0.98186 < 0.064071, 0.091158, 0.15344 >对应上面：q=0.98186 +0.064071i+0.091158j+0.15344 k
对一个四元数取模值
q.norm
ans =
1.0000求一个四元数的共轭，这个共轭也叫做逆四元数
>> q.inv()
ans =
0.98186 < -0.064071, -0.091158, -0.15344 >一个四元数乘以这个四元数的共轭（逆四元数）
>> q.inv()*q
ans =
1 < 0, 0, 0 >求一个四元数对应旋转矩阵
>> q.R
ans =
0.9363 -0.2896 0.1987
0.3130 0.9447 -0.0978
-0.1593 0.1538 0.9752四元数的规范化：
q=q.unit();变换矩阵，就是旋转矩阵+平移向量的齐次坐标表示：
如果你已经知道了一个变换矩阵T
那么可以通过t2r(T)提取出旋转矩阵
可以通过transl(T)提取出平移向量
惯性导航系统
惯性导航系统主要测量其加速度和角速度，并整合出他们的时间关系。
导航就是引导一个运载工具到达预定位置的方法。
要运行simulink模型，首先要加载他
sl_drivepoint设置初始的位姿
x0=[8 5 pi/2];设置目标的位置
xg=[5 5]开始仿真：
然后在命令窗口输入
sim('sl_drivepoint')或者用ctrl+T也可以达到相同的效果
同时我们需要有个变量应该来存储返回值，所以上面的命令应该写成
r=sim('sl_drivepoint');然后对于那些变量的值，我们可由find方法获得
t=r.find('yout')做出运动轨迹的曲线
plot(t(:,1),t(:,2))
网格占用法，它是把整个区域划分成若干网格，把每个网格分成占用或者非占用。用0表示非占用网格，也就是机器人可通行的区域。1表示占用网格，也就是不可通行区域或者说是障碍。网格的大小取决于具体应用。网格太小，计算机需要的存储的空间就要增加。
使用工具箱当中的makemap来创建地图
map=makemap(100);
bug算法运用的策略就是，它先让机器人沿着一条直线向目标点运动，如果遇到障碍物，机器人就逆时针绕行，
首先我们在这个工具箱当中创建一个bug2类
bug=Bug2(map);
%设置目标点
bug.goal=[50;35];
%用path进行路径仿真
p=bug.path([20;10]);在这本书当中默认goal=[50;30] start=[20;10];
>> dx=DXform(map);
>> dx.plan(goal);
>> dx.plot(p)
红色区域是我话的障碍物的区域
D*是一种流行的机器人路径规划的算法，有很多的实用特性。D*将占用网格推广为一张成本地图，其中c>0表示在水平方向或者垂直方向上穿越每个单元的成本。穿越斜对角的成本则是：
，那么对于障碍物
D*是通过寻找一条路径使得运行的总成本最低。
首先建立一个D*导航对象
ds=Dstar(map);将导入的地图转化成0，1 ，inf
c=ds.costmap_set;设置目标点起始点
ds.plan(goal);
ds.path(start)
航迹推演法
是一种基于预测速度、方向、运动时间以及前一个位置信息对下一个位置进行估计的方法。
一种描述机器人位置最有效的办法就是用概率密度函数PDF来表示机器人所有可能的位置。如下面图展示的一些概率密度函数的例子，在这些例子当中，函数的幅值对应机器人出现在该位置的相对可能性的大小
卡尔曼滤波器就是使用了一个递归算法，他在每个时间步长上都会根据上一次的估计值和噪声的观测值，给出下一个位置真实位姿的最优估计以及有关该估计的不确定性。