严老师matlab工具箱仿真-无人车定位

严老师matlab工具箱仿真-无人车定位

1. trj仿真测试

所用的轨迹仿真工具，为基于matlab的二次开发的开源工具包，默认模式为飞行器运动仿真．通过自定义运动模式和参数，生成相应的机载imu数据和姿态、速度、位置等．

1.1. 自定义运动模式

运动模式的定义是根据时间进行分段的，加入要模拟一段首钢园区的轨迹路线，按照园区的路线分布，自定义下面一段运动轨迹，从编号１到23段，时间分段的运动定义详细见下表：

分段编号	运动模式	相关参数	时长(s)
１	直线加速	角速度w=0,加速度a= 1m/s^2 , 向心加速度=false	10s
２	运动保持	角速度w=0, 加速度a=0, 向心加速度=false	15s
３	直线减速	角速度w=0, 加速度a=-1m/s^2, 向心加速度=false	5s
4-	复合左转	角速度w=18deg/s, 加速度a=0,向心加速度=true	10s
5	直线加速	角速度w=０, 加速度a= 1m/s^2 ,向心加速度=false	10s
６	运动保持	角速度w=0, 加速度a=0,向心加速度=false	10s
7	直线减速	角速度w=0, 加速度a=-1m/s^2,向心加速度=false	5s
8-	复合右转	角速度w=-9deg/s, 加速度a=0,向心加速度=true	10s
9	直线加速	角速度w=０, 加速度a=2m/s^2,向心加速度=false	5s
10	运动保持	角速度w=0, 加速度a=0,向心加速度=false	50s
11	直线减速	角速度w=0, 加速度a=-1m/s^2,向心加速度=false	5s
12-	复合左转	角速度w=9deg/s, 加速度a=0,向心加速度=true	10s
13	直线加速	角速度w=０, 加速度a=1m/s^2,向心加速度=false	5s
14	运动保持	角速度w=0, 加速度a=0,向心加速度=false	10s
15	直线减速	角速度w=0, 加速度a=-1.8m/s^2,向心加速度=false	10s
16-	复合左转	角速度w=18deg/s, 加速度a=0,向心加速度=true	5s
17	运动保持	角速度w=0, 加速度a=0,向心加速度=false	50s
18	直线加速	角速度w=０, 加速度a=1m/s^2,向心加速度=false	5s
19-	复合左转	角速度w=9deg/s, 加速度a=0,向心加速度=true	10s
20	运动保持	角速度w=0, 加速度a=0,向心加速度=false	58s
21-	复合右转	角速度w=-9deg/s, 加速度a=0,向心加速度=true	10s
22	直线减速	角速度w=0, 加速度a=-1m/s^2,向心加速度=false	5s
23	运动保持	角速度w=0, 加速度a=0,向心加速度=false	100s

注：复合左右转中，当飞机在做方位转弯的时候,需要先进行滚转一定角度,在进行转弯后在滚转回来.
% 对应的不同的方位转弯速度,需要滚转不同的角度,具体的计算方式 见捷联惯导算法与组合导航原理 -7.1.5

1.2. 仿真数据生成

1.2.1. 生成的imu数据，姿态，速度，位置．

设定数据采样间隔时间为0.01

1.2.2. 噪声设定

(0.03, 100, 0.001, 5)
%分别为　imuerr.eb为陀螺仪零偏稳定性，imuerr.db为加速度计零偏稳定性，imuerr.web为角速度随机游走，imuerr.wdb为加速度随机游走
%     eb - gyro constant bias (deg/h)
%     db - acc constant bias (ug)
%     web - angular random walk (deg/sqrt(h))
%     wdb - velocity random walk (ug/sqrt(Hz))

 imu = K*imu + drift error.
     drift = [ ts*imuerr.eb(1) + sts*imuerr.web(1)*randn(m,1), ...
              ts*imuerr.eb(2) + sts*imuerr.web(2)*randn(m,1), ...
              ts*imuerr.eb(3) + sts*imuerr.web(3)*randn(m,1), ...
              ts*imuerr.db(1) + sts*imuerr.wdb(1)*randn(m,1), ...
              ts*imuerr.db(2) + sts*imuerr.wdb(2)*randn(m,1), ...
              ts*imuerr.db(3) + sts*imuerr.wdb(3)*randn(m,1) ];

% GPS噪声设定　标准差为１m
 posGPS = trj.pos + randn(3,1); 

1.3. 定位算法仿真结果

讲上述生成的仿真原始数据，加上漂移和噪声后，输入定位算法进行测试：

roll,pitch,yaw姿态&Vn速度＆位置＆轨迹

**姿态＆位置误差 **