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

1. trj仿真测试

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

1.1. 自定义运动模式

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

分段编号运动模式相关参数时长(s)１直线加速角速度w=0,加速度a= 1m/s^2 , 向心加速度=false10s２运动保持角速度w=0, 加速度a=0, 向心加速度=false15s３直线减速角速度w=0, 加速度a=-1m/s^2, 向心加速度=false5s4-复合左转角速度w=18deg/s, 加速度a=0,向心加速度=true10s5直线加速角速度w=０, 加速度a= 1m/s^2 ,向心加速度=false10s６运动保持角速度w=0, 加速度a=0,向心加速度=false10s7直线减速角速度w=0, 加速度a=-1m/s^2,向心加速度=false5s8-复合右转角速度w=-9deg/s, 加速度a=0,向心加速度=true10s9直线加速角速度w=０, 加速度a=2m/s^2,向心加速度=false5s10运动保持角速度w=0, 加速度a=0,向心加速度=false50s11直线减速角速度w=0, 加速度a=-1m/s^2,向心加速度=false5s12-复合左转角速度w=9deg/s, 加速度a=0,向心加速度=true10s13直线加速角速度w=０, 加速度a=1m/s^2,向心加速度=false5s14运动保持角速度w=0, 加速度a=0,向心加速度=false10s15直线减速角速度w=0, 加速度a=-1.8m/s^2,向心加速度=false10s16-复合左转角速度w=18deg/s, 加速度a=0,向心加速度=true5s17运动保持角速度w=0, 加速度a=0,向心加速度=false50s18直线加速角速度w=０, 加速度a=1m/s^2,向心加速度=false5s19-复合左转角速度w=9deg/s, 加速度a=0,向心加速度=true10s20运动保持角速度w=0, 加速度a=0,向心加速度=false58s21-复合右转角速度w=-9deg/s, 加速度a=0,向心加速度=true10s22直线减速角速度w=0, 加速度a=-1m/s^2,向心加速度=false5s23运动保持角速度w=0, 加速度a=0,向心加速度=false100s

注：复合左右转中，当飞机在做方位转弯的时候,需要先进行滚转一定角度,在进行转弯后在滚转回来.
% 对应的不同的方位转弯速度,需要滚转不同的角度,具体的计算方式 见捷联惯导算法与组合导航原理 -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噪声设定　标准差为１mposGPS = trj.pos + randn(3,1); 
 

1.3. 定位算法仿真结果

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

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

**姿态＆位置误差 **