惯性导航技术综合实验
实验三 惯性导航综合实验
实验31 初始准实验
实验目
结合已采集标定惯性传感器数进行粗准解实现准程通较传感器数准结果进步认识惯性传感器性导航系统中重位实际工程设计中针应需求采取相应导航系统方案提供
二实验容
利加速度计输出计算系统初始姿态利陀螺输出信号计算航角利惯性传感器数计算结果
三实验系统组成
MEMS IMU（类型IMU）稳压电源数采集系统分析系统
四实验原理
惯导系统开始进行导航解算前需进行初始准水准质重力加速度作准基准准精度取决两水加速度计精度加速度计零位输出会造成水准误差方位准常方位罗准质球转角速度作准基准影响准精度素等效东陀螺漂移
(1)
中分前时刻俯仰角横滚角计算值
水准误差方位准误差示：
(2)
五实验步骤结果
1实验步骤：
采集静止状态水加速度计输出式计算均值
(3)
中前n加计输出均值前n1加计输出均值前时刻加计输出值
利加计均值计算系统初始俯仰角横滚角
(4)
中分前时刻俯仰角横滚角计算值
2实验结果分析：
21MIMS IMU加速度计信息计算
（1）俯仰角横滚角图：
（2）失准角：
22实验结果分析
计算基MIMS IMU静止时data2进行初始准data2定初始姿态角相差
六源程序
clear
clc
g 97803267714
aload(＇E＼郭凤玲＼chushiduizhun＼data2txt＇)
axa(4)＇
aya(5)＇
aza(6)＇
初始值ax0(1)ax(1)1000*g 转化单位mg转化ms^2
ay0(1)ay(1)1000*g
az0(1)az(1)1000*g
theta(1)asin(ay(1)az(1))
gama(1)asin(ax(1)az(1))
for i2120047
ax0(i)ax0(i1)+(ax(i)ax0(i1))i
ay0(i)ay0(i1)+(ay(i)ay0(i1))i
az0(i)az0(i1)+(az(i)az0(i1))i
theta(i)asin(ay0(i)az0(i))
gama(i)asin(ax0(i)az0(i))
end
detfaixmean(ay0)g
detfaiymean(ax0)g
t1120047
plot(ttheta＇r＇tgama＇b＇)
title(＇俯仰角横滚角＇)ylabel(＇弧度（rad）＇)
legend(＇俯仰角＇＇横滚角＇)
实验32 惯性导航静态实验
实验目
1掌握捷联惯导系统基工作原理
2掌握捷联惯导系统捷联解算方法
3解捷联惯导系统误差传递规律方程
二实验原理
捷联惯性导航系统（SINS）导航解算流程图1示程序初始化部分获SINS初始姿态阵初始位置矩阵初值四元数读取SINS数更新频率等SINS工作参数
图1 惯性导航原理
里λ分纬度度ψθγ分载体航俯仰横滚角理坐标系东北天坐标系
1．姿态计算
姿态矩阵：
(1)
(2)
位置矩阵：
(3)
中：
(4)
姿态四元数更新姿态四元数微分方程：
(5)
简写：
(6)
中：
解四元数微分方程：
(7)
式中： (8)
中T导航解算周期
化四元数更新姿态矩阵：
(9)
提取姿态角：
(10)
2．速度计算
列速度方程进行速度更新
（11）
式中
(12)
速度更新
(13)
式（11）求出加速度实际程序中进步提高解算精度姿态阵更新前分计算两次加速度梯形法求速度更新值
3．位置矩阵更新位置计算
(14)
式中：
(17)
解述微分方程解法：
(15)
提取纬度：
(16)
四实验设备
① 捷联惯性导航实验系统套
② 监控计算机台
五实验容结果
1．MIMS IMU系统导航计算
① IMU固定夹具IMU连夹具起静置桌面
② 调整稳压电源输出电压+8V关闭电源连接稳压电源IMU供电输入端连接IMU信号线USB232转接线监控计算机
③ 开监控计算机中监控软件
④ 开捷联惯导实验系统电源捷联惯导实验系统开始启动
⑤ 保持捷联惯性导航系统静止600秒记录实时输出数
⑥ 停止记录数利捷联解算方法计算纯惯性导航误差
MIMS IMU系统纯惯导导航结果：
（1）速度误差
（2）姿态误差
（2）位置误差
2．中低精度惯性导航系统导航仿真
① 实验老师定组中低精度IMU静态IMU采样数初始姿态数中前300秒加速度计采样计算初始航GPS双天线数出
② 利捷联解算方法计算纯惯性导航误差
中低精度惯性导航系统导航仿真结果：
（1） 位置误差：
（2） 姿态误差：
（3） 速度误差：
3．导航仿真结果分析
1MIMS IMU系统度误差fai误差theta误差Vx误差调试程序程中选取安装误差阵变化微情况姿态误差速度误差变化说明准确安装误差补偿阵重面MIMS IMU图形没补偿情况图形图形未列出面图形中精度导航系统误差中低精度导航系统纬度误差gama误差Vy误差MIMS IMU误差
六实验源程序
1MIMS静态捷联结算
clear all
clc
Qload(＇E＼惯性器件综合实验＼作业＼初始准＼data2txt＇) 惯性信息 Fbb Wib_bb
面捷联解算初始值计算
re6378393球半径
e129825球扁率
g0 97803267714
wie1504107*pi1803600球转角速率
vv[000]初始速度
latitude01163438*pi180初始度
longitude039977584*pi180初始纬度
h00初始高度
FibbQ(46)*pi3600180
WibbQ(13)1000*g0
fai0 8716*pi180 航
theta0 0158*pi180 俯仰
gama0 0325*pi180
陀螺仪安装误差补偿阵
Kg[09933 00013 00020
00033 09950 00026
00010 00022 09912 ]
kg0[00201
00537
00810]
加速度计误差补偿
Ka[09987 00001 00020
00001 09988 00004
00033 00015 09988 ]
ka0 [00018
00014
00009]
求初始姿态矩阵
Ctb [ cos(gama0)*cos(fai0)sin(gama0)*sin(theta0)*sin(fai0) cos(gama0)*sin(fai0)+sin(gama0)*sin(theta0)*cos(fai0) sin(gama0)*cos(theta0)
cos(theta0)*sin(fai0) cos(theta0)*cos(fai0) sin(theta0)
sin(gama0)*cos(fai0)+cos(gama0)*sin(theta0)*sin(fai0) sin(gama0)*sin(fai0)cos(gama0)*sin(theta0)*cos(fai0) cos(gama0) * cos(theta0)]
cbtCtb＇
Tcbt
设置四元素初始值
if T(32)>T(23)
q105*sqrt(1+T(11)T(22)T(33))
else
q1(05*sqrt(1+T(11)T(22)T(33)))
end
if T(13)>T(31)
q205*sqrt(1T(11)+T(22)T(33))
else
q2(05*sqrt(1T(11)+T(22)T(33)))
end
if T(21)>T(12)
q305*sqrt(1T(11)T(22)+T(33))
else
q3(05*sqrt(1T(11)T(22)+T(33)))
end
q0sqrt(1q1^2q2^2q3^2)
位置阵初始值
Cte[sin(latitude0)cos(latitude0)0
sin(longitude0)*cos(latitude0)sin(longitude0)*sin(latitude0)cos(longitude0)
cos(longitude0)*cos(latitude0)cos(longitude0)*sin(latitude0)sin(longitude0)]
rn6356803
rm6378254
2循环解算
delt0005
Ti0005
初始
速度
vxzeros(1200471)
vyzeros(1200471)
vzzeros(1200471)
姿态
theta2zeros(1200471)俯仰
psi2zeros(1200471)偏航
gamma2zeros(1200471)滚转
位置纬度
l2zeros(1200471)度
a2zeros(1200471)纬度
for i1120047
fbFibb(i)
fbfb＇
fbKa*fb+ka0
wib_bWibb(i)
wib_bwib_b＇
wib_bKg* wib_b+kg0
fpT*fb转换理坐标系
g更新
gg0*(1+00052884*sin(latitude0)*sin(latitude0)00000059*sin(2*latitude0)*sin(2*latitude0))
difVx fp(1) + (20 * wie * sin(latitude0) + vv(1) * tan(latitude0) rn) * vv(2)
difVy fp(2) (20 * wie * sin(latitude0) + vv(1)* tan(latitude0) rn) * vv(1)
dvvfp[00g]+[ difVx difVy 0]
vvvv+dvv*delt积分法
wett[vv(2)(rn)vv(1)(rm)vv(1)*tan(latitude0)(rm)]
Wiet[0wie*cos(latitude0)wie*sin(latitude0)]
wtbbwib_binv(T)*(wett+Wiet)
面进行位置矩阵修正
dcCte*[0wett(3)wett(2)
wett(3)0wett(1)
wett(2)wett(1)0]
CteCte+dc*delt
位置计算
latitude0asin(Cte(33))
if Cte(31)>0
longitude0atan(Cte(32)Cte(31))
elseif Cte(32)>0等书条件
longitude0atan(Cte(32)Cte(31))+pi
else
longitude0atan(Cte(32)Cte(31))pi
end
Q[q0q1q2q3]
dQ12*[0wtbb(1)wtbb(2)wtbb(3)
wtbb(1)0wtbb(3)wtbb(2)
wtbb(2)wtbb(3)0wtbb(1)
wtbb(3)wtbb(2)wtbb(1)0]*Q
QQ+dQ*delt
qq(sqrt(Q(1)^2+Q(2)^2+Q(3)^2+Q(4)^2))
q0Q(1)qq
q1Q(2)qq
q2Q(3)qq
q3Q(4)qq
求捷联矩阵修正四元法
T[q0^2+q1^2q2^2q3^22*(q1*q2q0*q3)2*(q1*q3+q0*q2)
2*(q1*q2+q0*q3)q0^2q1^2+q2^2q3^22*(q2*q3q0*q1)
2*(q1*q3q0*q2)2*(q2*q3+q0*q1)q0^2q1^2q2^2+q3^2]
面根捷联矩阵T计算姿态角yaw yroll
theta0asin(T(32))
yawmatan(T(12)T(22))
rollmatan(T(31)T(33))
if T(22)<0
fai0yawm+pi
else
if yawm>0
fai0yawm12*pi
else
fai0yawm
end
end
if T(33)>0
gama0rollm
else
if rollm<0
gama0rollm+pi
else
gama0rollmpi
end
end
收集信息
速度
vx(i)vv(1)
vy(i)vv(2)
vz(i)vv(3)
姿态
theta2(i)theta0俯仰
psi2(i)fai0偏航
gamma2(i)gama0滚转
位置纬度
l2(i)longitude0度
a2(i)latitude0纬度
end
t01200120046 200
figure
hold on
plot(tvx)grid on
xlabel(＇时间秒＇)ylabel(＇东速度 米秒＇)
hold off
figure
hold on
plot(tvy)grid on
xlabel(＇时间秒＇)ylabel(＇北速度 米秒＇)
hold off
figure
hold on
plot(tvz)grid on
xlabel(＇时间秒＇)ylabel(＇天速度 米秒＇)
hold off
figure
hold on
plot(ttheta2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇俯仰角 度＇)
hold off
figure
hold on
plot(tpsi2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇偏航角 度＇)
hold off
figure
hold on
plot(tgamma2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇滚转角 度＇)
hold off
figure
hold on
plot(tl2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇度 度＇)
hold off
figure
hold on
plot(ta2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇纬度 度＇)
hold off
2 中低精度源程序
纯惯导解算中精度IMU静态数
clear
clc
close all
tic
初始量定义
pi 31415926535897931
wie 0000072921151467
Re 6378135072
g 97803267714
e 10 29825
初始理位置速度
lat_s 3997885*pi180 初始纬度
lon_s 116346824*pi180 度
Vx_s 0 东速
Vy_s 0 北速
调部分
datanumber174400 MIMS IMU导航结果截取相长度数进行处理
T001
已知角度(初始姿态)
fai_s 86813325*pi180 航
theta_s 0153277*pi180 俯仰
gama_s 0237267*pi180 横滚
a load(＇data4txt＇)
f a(911)＇ 三轴力输出单位ms^2
w0 a(2729)＇ 陀螺仪输出角速率信息单位脉数rp10ms
陀螺标定
tga(68)＇
GxtFtg(1)
GytFtg(2)
GztFtg(3)
kgx200003066*000655288964
kgx1003026*000655288964
kgx000268*000655288964
kgy200008726*00067110686
kgy1004716*00067110686
kgy000494*00067110686 脉数
kgz200006019*000656632715
kgz10005303*000656632715
kgz004919*000656632715 00040385312531699899
for i1datanumber
GxtGxtF(i)*GxtF(i)
GytGytF(i)*GytF(i)
GztGztF(i)*GztF(i)
end
bias_gx (kgx2*Gxt + kgx1*(GxtF) + kgx0)
bias_gy (kgy2*Gyt+ kgy1*(GytF) + kgy0)
bias_gz (kgz2*Gzt+ kgz1*(GztF) + kgz0)
ww0 w0(1) bias_gx
ww1 w0(2) bias_gy
ww2 w0(3) bias_gz
para pi1800
egxx0000423887215768231 (°rp)
egyy0000413897587790739
egzz0000423033091235588
egxy263723878994696e006 单位：弧度
egxz957100499531451e007
egyx203721501416524e006
egyz853961364243771e007
egzx439813070640871e007
egzy181987993694005e007
w(1) (ww0*egxx + egxy*ww1 + egxz*ww2)*para001 单位rads
w(2) (ww1*egyy + egyx*ww0 + egyz*ww2)*para001
w(3) (ww2*egzz + egzx*ww0 + egzy*ww1)*para001
惯导解算
Cnb [ cos(gama_s)*cos(fai_s)sin(gama_s)*sin(theta_s)*sin(fai_s) cos(gama_s)*sin(fai_s)+sin(gama_s)*sin(theta_s)*cos(fai_s) sin(gama_s)*cos(theta_s)
cos(theta_s)*sin(fai_s) cos(theta_s)*cos(fai_s) sin(theta_s)
sin(gama_s)*cos(fai_s)+cos(gama_s)*sin(theta_s)*sin(fai_s) sin(gama_s)*sin(fai_s)cos(gama_s)*sin(theta_s)*cos(fai_s) cos(gama_s) * cos(theta_s)]
q [ cos(fai_s2)*cos(theta_s2)*cos(gama_s2) sin(fai_s2)*sin(theta_s2)*sin(gama_s2)
cos(fai_s2)*sin(theta_s2)*cos(gama_s2) sin(fai_s2)*cos(theta_s2)*sin(gama_s2)
cos(fai_s2)*cos(theta_s2)*sin(gama_s2) + sin(fai_s2)*sin(theta_s2)*cos(gama_s2)
cos(fai_s2)*sin(theta_s2)*sin(gama_s2) + sin(fai_s2)*cos(theta_s2)*cos(gama_s2)]
gyrozeros(31)
acczeros(31)
pos_s zeros(datanumber52) 纯惯导结算轨迹
atti_s zeros(datanumber53)
v_s zeros(datanumber52)
for i11datanumber
Rmh Re * (10 20 * e + 30 * e * sin(lat_s) * sin(lat_s))
Rnh Re * (10 + e * sin(lat_s) * sin(lat_s))
Wien [ 0 wie * cos(lat_s) wie * sin(lat_s)]
Wenn [ Vy_s Rmh Vx_s Rnh Vx_s * tan(lat_s) Rnh]
Winn Wien + Wenn
Winb Cnb * Winn
for j13
gyro(j1) w(ji) 陀螺信息
acc(j1) f(ji) 加速度信息
end
angle (gyro Winb) * T
fn Cnb＇* acc
difVx fn(1) + (20 * wie * sin(lat_s) + Vx_s * tan(lat_s) Rnh) * Vy_s
difVy fn(2) (20 * wie * sin(lat_s) + Vx_s * tan(lat_s) Rnh) * Vx_s
Vx_s difVx * T + Vx_s
Vy_s difVy * T + Vy_s
if(mod(i5)0)
v_s(i5)[Vx_sVy_s]
a1(i51)a(i22) 度
a1(i52)a(i23) 纬度
a1(i53)a(i19) 航
a1(i54)a(i20) 俯仰
a1(i55)a(i21) 横滚
end
lat_s lat_s + Vy_s * T Rmh
lon_s lon_s + Vx_s * T Rnh cos(lat_s)
if(mod(i5)0)
pos_s(i5)[lat_slon_s]
end
M [0 angle(1) angle(2) angle(3)
angle(1) 0 angle(3) angle(2)
angle(2) angle(3) 0 angle(1)
angle(3) angle(2) angle(1) 0]
q (cos(norm(angle) 2) * eye(4) + sin(norm(angle) 2) norm(angle) * M) * q
q q norm(q)
Cnb [ q(1)*q(1)+q(2)*q(2)q(3)*q(3)q(4)*q(4) 2*(q(2)*q(3)+q(1)*q(4)) 2*(q(2)*q(4)q(1)*q(3))
2*(q(2)*q(3)q(1)*q(4)) q(1)*q(1)q(2)*q(2)+q(3)*q(3)q(4)*q(4) 2*(q(3)*q(4)+q(1)*q(2))
2*(q(2)*q(4)+q(1)*q(3)) 2*(q(3)*q(4)q(1)*q(2)) q(1)*q(1)q(2)*q(2)q(3)*q(3)+q(4)*q(4)]
fai_s atan(Cnb(21) Cnb(22))
theta_s asin(Cnb(23))
gama_s atan(Cnb(13) Cnb(33))
if (Cnb(22) < 0)
fai_s fai_s + pi
elseif (fai_s < 0)
fai_s fai_s + 2*pi
end
if (Cnb(33) < 0)
if(gama_s > 0)
gama_s gama_s pi
else
gama_s gama_s + pi
end
end
if(mod(i5)0)
atti_s(i5) [fai_spi*180 theta_spi*180 gama_spi*180]
end
i
end
绘图
t1datanumber5
figure(1)
subplot(211)
plot(tpos_s(1)*180pia1(2)＇b＇)
title(＇纬度误差＇)xlabel(＇005s＇)ylabel(＇度＇)
subplot(212)
plot(tpos_s(2)*180pi a1(1)＇b＇)
title(＇度误差＇)xlabel(＇005s＇)ylabel(＇度＇)
figure(2)
subplot(311)
plot(tatti_s(1)a1(3)＇b＇)
title(＇fai误差＇)xlabel(＇005s＇)ylabel(＇度＇)
subplot(312)
plot(tatti_s(2)a1(4)＇b＇)
title(＇theta误差＇)xlabel(＇005s＇)ylabel(＇度＇)
subplot(313)
plot(tatti_s(3)a1(5)＇b＇)
title(＇gama误差＇)xlabel(＇005s＇)ylabel(＇度＇)
figure(3)
subplot(211)
plot(tv_s(1)＇b＇)
title(＇Vx＇)xlabel(＇005s＇)ylabel(＇ms＇)
subplot(212)
plot(tv_s(2)＇b＇)
title(＇Vy＇)xlabel(＇005s＇)ylabel(＇ms＇)
toc
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档
惯性导航技术综合实验
实验三 惯性导航综合实验
实验31 初始准实验
实验目
结合已采集标定惯性传感器数进行粗准解实现准程通较传感器数准结果进步认识惯性传感器性导航系统中重位实际工程设计中针应需求采取相应导航系统方案提供
二实验容
利加速度计输出计算系统初始姿态利陀螺输出信号计算航角利惯性传感器数计算结果
三实验系统组成
MEMS IMU（类型IMU）稳压电源数采集系统分析系统
四实验原理
惯导系统开始进行导航解算前需进行初始准水准质重力加速度作准基准准精度取决两水加速度计精度加速度计零位输出会造成水准误差方位准常方位罗准质球转角速度作准基准影响准精度素等效东陀螺漂移
(1)
中分前时刻俯仰角横滚角计算值
水准误差方位准误差示：
(2)
五实验步骤结果
1实验步骤：
采集静止状态水加速度计输出式计算均值
(3)
中前n加计输出均值前n1加计输出均值前时刻加计输出值
利加计均值计算系统初始俯仰角横滚角
(4)
中分前时刻俯仰角横滚角计算值
2实验结果分析：
21MIMS IMU加速度计信息计算
（1）俯仰角横滚角图：
（2）失准角：
22实验结果分析
计算基MIMS IMU静止时data2进行初始准data2定初始姿态角相差
六源程序
clear
clc
g 97803267714
aload(＇E＼郭凤玲＼chushiduizhun＼data2txt＇)
axa(4)＇
aya(5)＇
aza(6)＇
初始值ax0(1)ax(1)1000*g 转化单位mg转化ms^2
ay0(1)ay(1)1000*g
az0(1)az(1)1000*g
theta(1)asin(ay(1)az(1))
gama(1)asin(ax(1)az(1))
for i2120047
ax0(i)ax0(i1)+(ax(i)ax0(i1))i
ay0(i)ay0(i1)+(ay(i)ay0(i1))i
az0(i)az0(i1)+(az(i)az0(i1))i
theta(i)asin(ay0(i)az0(i))
gama(i)asin(ax0(i)az0(i))
end
detfaixmean(ay0)g
detfaiymean(ax0)g
t1120047
plot(ttheta＇r＇tgama＇b＇)
title(＇俯仰角横滚角＇)ylabel(＇弧度（rad）＇)
legend(＇俯仰角＇＇横滚角＇)
实验32 惯性导航静态实验
实验目
1掌握捷联惯导系统基工作原理
2掌握捷联惯导系统捷联解算方法
3解捷联惯导系统误差传递规律方程
二实验原理
捷联惯性导航系统（SINS）导航解算流程图1示程序初始化部分获SINS初始姿态阵初始位置矩阵初值四元数读取SINS数更新频率等SINS工作参数
图1 惯性导航原理
里λ分纬度度ψθγ分载体航俯仰横滚角理坐标系东北天坐标系
1．姿态计算
姿态矩阵：
(1)
(2)
位置矩阵：
(3)
中：
(4)
姿态四元数更新姿态四元数微分方程：
(5)
简写：
(6)
中：
解四元数微分方程：
(7)
式中： (8)
中T导航解算周期
化四元数更新姿态矩阵：
(9)
提取姿态角：
(10)
2．速度计算
列速度方程进行速度更新
（11）
式中
(12)
速度更新
(13)
式（11）求出加速度实际程序中进步提高解算精度姿态阵更新前分计算两次加速度梯形法求速度更新值
3．位置矩阵更新位置计算
(14)
式中：
(17)
解述微分方程解法：
(15)
提取纬度：
(16)
四实验设备
① 捷联惯性导航实验系统套
② 监控计算机台
五实验容结果
1．MIMS IMU系统导航计算
① IMU固定夹具IMU连夹具起静置桌面
② 调整稳压电源输出电压+8V关闭电源连接稳压电源IMU供电输入端连接IMU信号线USB232转接线监控计算机
③ 开监控计算机中监控软件
④ 开捷联惯导实验系统电源捷联惯导实验系统开始启动
⑤ 保持捷联惯性导航系统静止600秒记录实时输出数
⑥ 停止记录数利捷联解算方法计算纯惯性导航误差
MIMS IMU系统纯惯导导航结果：
（1）速度误差
（2）姿态误差
（2）位置误差
2．中低精度惯性导航系统导航仿真
① 实验老师定组中低精度IMU静态IMU采样数初始姿态数中前300秒加速度计采样计算初始航GPS双天线数出
② 利捷联解算方法计算纯惯性导航误差
中低精度惯性导航系统导航仿真结果：
（1） 位置误差：
（2） 姿态误差：
（3） 速度误差：
3．导航仿真结果分析
1MIMS IMU系统度误差fai误差theta误差Vx误差调试程序程中选取安装误差阵变化微情况姿态误差速度误差变化说明准确安装误差补偿阵重面MIMS IMU图形没补偿情况图形图形未列出面图形中精度导航系统误差中低精度导航系统纬度误差gama误差Vy误差MIMS IMU误差
六实验源程序
1MIMS静态捷联结算
clear all
clc
Qload(＇E＼惯性器件综合实验＼作业＼初始准＼data2txt＇) 惯性信息 Fbb Wib_bb
面捷联解算初始值计算
re6378393球半径
e129825球扁率
g0 97803267714
wie1504107*pi1803600球转角速率
vv[000]初始速度
latitude01163438*pi180初始度
longitude039977584*pi180初始纬度
h00初始高度
FibbQ(46)*pi3600180
WibbQ(13)1000*g0
fai0 8716*pi180 航
theta0 0158*pi180 俯仰
gama0 0325*pi180
陀螺仪安装误差补偿阵
Kg[09933 00013 00020
00033 09950 00026
00010 00022 09912 ]
kg0[00201
00537
00810]
加速度计误差补偿
Ka[09987 00001 00020
00001 09988 00004
00033 00015 09988 ]
ka0 [00018
00014
00009]
求初始姿态矩阵
Ctb [ cos(gama0)*cos(fai0)sin(gama0)*sin(theta0)*sin(fai0) cos(gama0)*sin(fai0)+sin(gama0)*sin(theta0)*cos(fai0) sin(gama0)*cos(theta0)
cos(theta0)*sin(fai0) cos(theta0)*cos(fai0) sin(theta0)
sin(gama0)*cos(fai0)+cos(gama0)*sin(theta0)*sin(fai0) sin(gama0)*sin(fai0)cos(gama0)*sin(theta0)*cos(fai0) cos(gama0) * cos(theta0)]
cbtCtb＇
Tcbt
设置四元素初始值
if T(32)>T(23)
q105*sqrt(1+T(11)T(22)T(33))
else
q1(05*sqrt(1+T(11)T(22)T(33)))
end
if T(13)>T(31)
q205*sqrt(1T(11)+T(22)T(33))
else
q2(05*sqrt(1T(11)+T(22)T(33)))
end
if T(21)>T(12)
q305*sqrt(1T(11)T(22)+T(33))
else
q3(05*sqrt(1T(11)T(22)+T(33)))
end
q0sqrt(1q1^2q2^2q3^2)
位置阵初始值
Cte[sin(latitude0)cos(latitude0)0
sin(longitude0)*cos(latitude0)sin(longitude0)*sin(latitude0)cos(longitude0)
cos(longitude0)*cos(latitude0)cos(longitude0)*sin(latitude0)sin(longitude0)]
rn6356803
rm6378254
2循环解算
delt0005
Ti0005
初始
速度
vxzeros(1200471)
vyzeros(1200471)
vzzeros(1200471)
姿态
theta2zeros(1200471)俯仰
psi2zeros(1200471)偏航
gamma2zeros(1200471)滚转
位置纬度
l2zeros(1200471)度
a2zeros(1200471)纬度
for i1120047
fbFibb(i)
fbfb＇
fbKa*fb+ka0
wib_bWibb(i)
wib_bwib_b＇
wib_bKg* wib_b+kg0
fpT*fb转换理坐标系
g更新
gg0*(1+00052884*sin(latitude0)*sin(latitude0)00000059*sin(2*latitude0)*sin(2*latitude0))
difVx fp(1) + (20 * wie * sin(latitude0) + vv(1) * tan(latitude0) rn) * vv(2)
difVy fp(2) (20 * wie * sin(latitude0) + vv(1)* tan(latitude0) rn) * vv(1)
dvvfp[00g]+[ difVx difVy 0]
vvvv+dvv*delt积分法
wett[vv(2)(rn)vv(1)(rm)vv(1)*tan(latitude0)(rm)]
Wiet[0wie*cos(latitude0)wie*sin(latitude0)]
wtbbwib_binv(T)*(wett+Wiet)
面进行位置矩阵修正
dcCte*[0wett(3)wett(2)
wett(3)0wett(1)
wett(2)wett(1)0]
CteCte+dc*delt
位置计算
latitude0asin(Cte(33))
if Cte(31)>0
longitude0atan(Cte(32)Cte(31))
elseif Cte(32)>0等书条件
longitude0atan(Cte(32)Cte(31))+pi
else
longitude0atan(Cte(32)Cte(31))pi
end
Q[q0q1q2q3]
dQ12*[0wtbb(1)wtbb(2)wtbb(3)
wtbb(1)0wtbb(3)wtbb(2)
wtbb(2)wtbb(3)0wtbb(1)
wtbb(3)wtbb(2)wtbb(1)0]*Q
QQ+dQ*delt
qq(sqrt(Q(1)^2+Q(2)^2+Q(3)^2+Q(4)^2))
q0Q(1)qq
q1Q(2)qq
q2Q(3)qq
q3Q(4)qq
求捷联矩阵修正四元法
T[q0^2+q1^2q2^2q3^22*(q1*q2q0*q3)2*(q1*q3+q0*q2)
2*(q1*q2+q0*q3)q0^2q1^2+q2^2q3^22*(q2*q3q0*q1)
2*(q1*q3q0*q2)2*(q2*q3+q0*q1)q0^2q1^2q2^2+q3^2]
面根捷联矩阵T计算姿态角yaw yroll
theta0asin(T(32))
yawmatan(T(12)T(22))
rollmatan(T(31)T(33))
if T(22)<0
fai0yawm+pi
else
if yawm>0
fai0yawm12*pi
else
fai0yawm
end
end
if T(33)>0
gama0rollm
else
if rollm<0
gama0rollm+pi
else
gama0rollmpi
end
end
收集信息
速度
vx(i)vv(1)
vy(i)vv(2)
vz(i)vv(3)
姿态
theta2(i)theta0俯仰
psi2(i)fai0偏航
gamma2(i)gama0滚转
位置纬度
l2(i)longitude0度
a2(i)latitude0纬度
end
t01200120046 200
figure
hold on
plot(tvx)grid on
xlabel(＇时间秒＇)ylabel(＇东速度 米秒＇)
hold off
figure
hold on
plot(tvy)grid on
xlabel(＇时间秒＇)ylabel(＇北速度 米秒＇)
hold off
figure
hold on
plot(tvz)grid on
xlabel(＇时间秒＇)ylabel(＇天速度 米秒＇)
hold off
figure
hold on
plot(ttheta2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇俯仰角 度＇)
hold off
figure
hold on
plot(tpsi2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇偏航角 度＇)
hold off
figure
hold on
plot(tgamma2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇滚转角 度＇)
hold off
figure
hold on
plot(tl2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇度 度＇)
hold off
figure
hold on
plot(ta2*180pi)grid on
xlabel(＇时间秒＇)ylabel(＇纬度 度＇)
hold off
2 中低精度源程序
纯惯导解算中精度IMU静态数
clear
clc
close all
tic
初始量定义
pi 31415926535897931
wie 0000072921151467
Re 6378135072
g 97803267714
e 10 29825
初始理位置速度
lat_s 3997885*pi180 初始纬度
lon_s 116346824*pi180 度
Vx_s 0 东速
Vy_s 0 北速
调部分
datanumber174400 MIMS IMU导航结果截取相长度数进行处理
T001
已知角度(初始姿态)
fai_s 86813325*pi180 航
theta_s 0153277*pi180 俯仰
gama_s 0237267*pi180 横滚
a load(＇data4txt＇)
f a(911)＇ 三轴力输出单位ms^2
w0 a(2729)＇ 陀螺仪输出角速率信息单位脉数rp10ms
陀螺标定
tga(68)＇
GxtFtg(1)
GytFtg(2)
GztFtg(3)
kgx200003066*000655288964
kgx1003026*000655288964
kgx000268*000655288964
kgy200008726*00067110686
kgy1004716*00067110686
kgy000494*00067110686 脉数
kgz200006019*000656632715
kgz10005303*000656632715
kgz004919*000656632715 00040385312531699899
for i1datanumber
GxtGxtF(i)*GxtF(i)
GytGytF(i)*GytF(i)
GztGztF(i)*GztF(i)
end
bias_gx (kgx2*Gxt + kgx1*(GxtF) + kgx0)
bias_gy (kgy2*Gyt+ kgy1*(GytF) + kgy0)
bias_gz (kgz2*Gzt+ kgz1*(GztF) + kgz0)
ww0 w0(1) bias_gx
ww1 w0(2) bias_gy
ww2 w0(3) bias_gz
para pi1800
egxx0000423887215768231 (°rp)
egyy0000413897587790739
egzz0000423033091235588
egxy263723878994696e006 单位：弧度
egxz957100499531451e007
egyx203721501416524e006
egyz853961364243771e007
egzx439813070640871e007
egzy181987993694005e007
w(1) (ww0*egxx + egxy*ww1 + egxz*ww2)*para001 单位rads
w(2) (ww1*egyy + egyx*ww0 + egyz*ww2)*para001
w(3) (ww2*egzz + egzx*ww0 + egzy*ww1)*para001
惯导解算
Cnb [ cos(gama_s)*cos(fai_s)sin(gama_s)*sin(theta_s)*sin(fai_s) cos(gama_s)*sin(fai_s)+sin(gama_s)*sin(theta_s)*cos(fai_s) sin(gama_s)*cos(theta_s)
cos(theta_s)*sin(fai_s) cos(theta_s)*cos(fai_s) sin(theta_s)
sin(gama_s)*cos(fai_s)+cos(gama_s)*sin(theta_s)*sin(fai_s) sin(gama_s)*sin(fai_s)cos(gama_s)*sin(theta_s)*cos(fai_s) cos(gama_s) * cos(theta_s)]
q [ cos(fai_s2)*cos(theta_s2)*cos(gama_s2) sin(fai_s2)*sin(theta_s2)*sin(gama_s2)
cos(fai_s2)*sin(theta_s2)*cos(gama_s2) sin(fai_s2)*cos(theta_s2)*sin(gama_s2)
cos(fai_s2)*cos(theta_s2)*sin(gama_s2) + sin(fai_s2)*sin(theta_s2)*cos(gama_s2)
cos(fai_s2)*sin(theta_s2)*sin(gama_s2) + sin(fai_s2)*cos(theta_s2)*cos(gama_s2)]
gyrozeros(31)
acczeros(31)
pos_s zeros(datanumber52) 纯惯导结算轨迹
atti_s zeros(datanumber53)
v_s zeros(datanumber52)
for i11datanumber
Rmh Re * (10 20 * e + 30 * e * sin(lat_s) * sin(lat_s))
Rnh Re * (10 + e * sin(lat_s) * sin(lat_s))
Wien [ 0 wie * cos(lat_s) wie * sin(lat_s)]
Wenn [ Vy_s Rmh Vx_s Rnh Vx_s * tan(lat_s) Rnh]
Winn Wien + Wenn
Winb Cnb * Winn
for j13
gyro(j1) w(ji) 陀螺信息
acc(j1) f(ji) 加速度信息
end
angle (gyro Winb) * T
fn Cnb＇* acc
difVx fn(1) + (20 * wie * sin(lat_s) + Vx_s * tan(lat_s) Rnh) * Vy_s
difVy fn(2) (20 * wie * sin(lat_s) + Vx_s * tan(lat_s) Rnh) * Vx_s
Vx_s difVx * T + Vx_s
Vy_s difVy * T + Vy_s
if(mod(i5)0)
v_s(i5)[Vx_sVy_s]
a1(i51)a(i22) 度
a1(i52)a(i23) 纬度
a1(i53)a(i19) 航
a1(i54)a(i20) 俯仰
a1(i55)a(i21) 横滚
end
lat_s lat_s + Vy_s * T Rmh
lon_s lon_s + Vx_s * T Rnh cos(lat_s)
if(mod(i5)0)
pos_s(i5)[lat_slon_s]
end
M [0 angle(1) angle(2) angle(3)
angle(1) 0 angle(3) angle(2)
angle(2) angle(3) 0 angle(1)
angle(3) angle(2) angle(1) 0]
q (cos(norm(angle) 2) * eye(4) + sin(norm(angle) 2) norm(angle) * M) * q
q q norm(q)
Cnb [ q(1)*q(1)+q(2)*q(2)q(3)*q(3)q(4)*q(4) 2*(q(2)*q(3)+q(1)*q(4)) 2*(q(2)*q(4)q(1)*q(3))
2*(q(2)*q(3)q(1)*q(4)) q(1)*q(1)q(2)*q(2)+q(3)*q(3)q(4)*q(4) 2*(q(3)*q(4)+q(1)*q(2))
2*(q(2)*q(4)+q(1)*q(3)) 2*(q(3)*q(4)q(1)*q(2)) q(1)*q(1)q(2)*q(2)q(3)*q(3)+q(4)*q(4)]
fai_s atan(Cnb(21) Cnb(22))
theta_s asin(Cnb(23))
gama_s atan(Cnb(13) Cnb(33))
if (Cnb(22) < 0)
fai_s fai_s + pi
elseif (fai_s < 0)
fai_s fai_s + 2*pi
end
if (Cnb(33) < 0)
if(gama_s > 0)
gama_s gama_s pi
else
gama_s gama_s + pi
end
end
if(mod(i5)0)
atti_s(i5) [fai_spi*180 theta_spi*180 gama_spi*180]
end
i
end
绘图
t1datanumber5
figure(1)
subplot(211)
plot(tpos_s(1)*180pia1(2)＇b＇)
title(＇纬度误差＇)xlabel(＇005s＇)ylabel(＇度＇)
subplot(212)
plot(tpos_s(2)*180pi a1(1)＇b＇)
title(＇度误差＇)xlabel(＇005s＇)ylabel(＇度＇)
figure(2)
subplot(311)
plot(tatti_s(1)a1(3)＇b＇)
title(＇fai误差＇)xlabel(＇005s＇)ylabel(＇度＇)
subplot(312)
plot(tatti_s(2)a1(4)＇b＇)
title(＇theta误差＇)xlabel(＇005s＇)ylabel(＇度＇)
subplot(313)
plot(tatti_s(3)a1(5)＇b＇)
title(＇gama误差＇)xlabel(＇005s＇)ylabel(＇度＇)
figure(3)
subplot(211)
plot(tv_s(1)＇b＇)
title(＇Vx＇)xlabel(＇005s＇)ylabel(＇ms＇)
subplot(212)
plot(tv_s(2)＇b＇)
title(＇Vy＇)xlabel(＇005s＇)ylabel(＇ms＇)
toc
文档香网(httpswwwxiangdangnet)户传
《香当网》用户分享的内容，不代表《香当网》观点或立场，请自行判断内容的真实性和可靠性！
该内容是文档的文本内容，更好的格式请下载文档