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

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

1. trj仿真测试

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

1.1. 自定义运动模式

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

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

**姿态＆位置误差 **