高、低成本MEMS惯导系统姿态、位置、速度更新算法的对比

news2024/11/24 7:52:23

高、低成本MEMS惯导系统姿态、位置、速度更新算法的对比

  • 一、高成本MEMS惯导系统姿态、位置、速度更新算法
    • 1、速度更新
    • 2、位置更新
    • 3、姿态更新
    • 4、程序仿真及实验结果
      • 4.1 主函数
      • 4.2 子函数
      • 4.3 实验结果
  • 一、低成本MEMS惯导系统姿态、位置、速度更新算法
    • 1、速度更新
    • 2、位置更新
    • 3、姿态更新
    • 4、程序仿真及实验结果
      • 4.1 主函数
      • 4.2 子函数
      • 4.3 实验结果
        • 1. 不考虑地球自转的影响
        • 2. 考虑地球自转的影响
    • 5. 程序代码及数据下载地址

一、高成本MEMS惯导系统姿态、位置、速度更新算法

1、速度更新

 wie= 7.2921151467e-5;   %自传角速度 wgs-84 model
 eth.sl = sin(pos(1));  eth.cl = cos(pos(1));  eth.tl = eth.sl/eth.cl;  % eth.sl -- sinL；eth.cl -- cosL；eth.tl -- tanL;
 eth.RMh = Re*(1-e2)/sq/sq2+pos(3);  % 子午圈主曲率半径+飞行高度   （3.1.23）
 eth.RNh = Re/sq2+pos(3);  eth.clRNh = eth.cl*eth.RNh;  % 卯酉圈主曲率半径+飞行高度   （3.1.24）
 eth.wnie = wie*[0; eth.cl; eth.sl];  % 地球自转角速度三个分量值x y z
 eth.wnen = [-vn(2)/eth.RMh; vn(1)/eth.RNh; vn(1)/eth.RNh*eth.tl];  % 载体运动的角速度分量，设定东“+”西“-”。 （3.1.24）
 eth.wnin = eth.wnie + eth.wnen;  % 载体相对惯性系的角速度; eth.wnie -- 地球自转引起的导航系旋转  eth.wnen -- 惯导系统在地球表面附近移动因地球表面弯曲引起的n系旋转;
 [phim, dvbm] = cnscl(wm, vm);  % 圆锥误差/划船误差补偿；phim -- 补偿后旋转矢量；dvbm -- 补偿后速度增量dvbm = (vmm+0.5*cross(wmm,vmm)+scullm)';
 eth.gcc = eth.gn - cross(eth.wnien,vn); % 考虑重力/哥氏力/向心力
 nn = 2; ts = 0.1; nts = nn*ts; % nn -- 子样数；ts -- 采样时间间隔；nts -- 采样周期

 % 速度更新
 vn1 = vn + rv2m(-eth.wnin*nts/2)*qmulv(qnb,dvbm) + eth.gcc*nts;  %  rv2m -- 等效旋转矢量转换为方向余弦；qmulv -- 四元数乘矢量，即三维矢量的四元数坐标变换(2.4-26)

v m n ( m ) = v m − 1 n ( m − 1 ) + Δ v s f ( m ) n + Δ v c o r / g ( m ) n {\bf{v}}_m^{n(m)} = {\bf{v}}_{m - 1}^{n(m - 1)} + \Delta {\bf{v}}_{{\rm{sf}}(m)}^n + \Delta {\bf{v}}_{{\rm{cor/g}}(m)}^n vmn(m)​=vm−1n(m−1)​+Δvsf(m)n​+Δvcor/g(m)n​
Δ v s f ( m ) n ≈ [ I − T 2 ( ω i n ( m − 1 / 2 ) n × ) ] C b ( m − 1 ) n ( m − 1 ) ( Δ v m + Δ v r o t ( m ) b ( m − 1 ) + Δ v s c u l ( m ) b ( m − 1 ) ) \Delta {\bf{v}}_{{\rm{sf}}(m)}^n \approx \left[ {{\bf{I}} - \frac{T}{2}({\bf{\omega }}_{in(m - 1/2)}^n \times )} \right]{\bf{C}}_{b(m - 1)}^{n(m - 1)}(\Delta {{\bf{v}}_m} + \Delta {\bf{v}}_{{\rm{rot}}(m)}^{b(m - 1)}{\rm{ + }}\Delta {\bf{v}}_{{\rm{scul}}(m)}^{b(m - 1)}) Δvsf(m)n​≈[I−2T​(ωin(m−1/2)n​×)]Cb(m−1)n(m−1)​(Δvm​+Δvrot(m)b(m−1)​+Δvscul(m)b(m−1)​)

Δ v c o r / g ( m ) n ≈ { − [ 2 ω i e ( m − 1 / 2 ) n + ω e n ( m − 1 / 2 ) n ] × v m − 1 / 2 n + g m − 1 / 2 n } T \Delta {\bf{v}}_{{\rm{cor/g}}(m)}^n \approx \left\{ { - \left[ {2{\bf{\omega }}_{ie(m - 1/2)}^n + {\bf{\omega }}_{en(m - 1/2)}^n} \right] \times {\bf{v}}_{m - 1/2}^n + {\bf{g}}_{m - 1/2}^n} \right\}T Δvcor/g(m)n​≈{−[2ωie(m−1/2)n​+ωen(m−1/2)n​]×vm−1/2n​+gm−1/2n​}T

符号含义 / 程序中表现为
Δ v s f ( m ) n \Delta {\bf{v}}_{{\rm{sf}}(m)}^n Δvsf(m)n​导航系比力速度增量
Δ v c o r / g ( m ) n \Delta {\bf{v}}_{{\rm{cor/g}}(m)}^n Δvcor/g(m)n​有害加速度的速度增量（4.1-27）
Δ v r o t ( m ) b ( m − 1 ) \Delta {\bf{v}}_{{\rm{rot}}(m)}^{b(m - 1)} Δvrot(m)b(m−1)​速度的旋转误差补偿量
Δ v s c u l ( m ) b ( m − 1 ) \Delta {\bf{v}}_{{\rm{scul}}(m)}^{b(m - 1)} Δvscul(m)b(m−1)​二子样速度划桨误差补偿量
( Δ v m + Δ v r o t ( m ) b ( m − 1 ) + Δ v s c u l ( m ) b ( m − 1 ) ) (\Delta {{\bf{v}}_m} + \Delta {\bf{v}}_{{\rm{rot}}(m)}^{b(m - 1)}{\rm{ + }}\Delta {\bf{v}}_{{\rm{scul}}(m)}^{b(m - 1)}) (Δvm​+Δvrot(m)b(m−1)​+Δvscul(m)b(m−1)​)dvbm – 补偿后速度增量dvbm = (vmm+0.5*cross(wmm,vmm)+scullm)';
[ I − T 2 ( ω i n ( m − 1 / 2 ) n × ) ] \left[ {{\bf{I}} - \frac{T}{2}({\bf{\omega }}_{in(m - 1/2)}^n \times )} \right] [I−2T​(ωin(m−1/2)n​×)]rv2m(-eth.wnin*nts/2)
{ − [ 2 ω i e ( m − 1 / 2 ) n + ω e n ( m − 1 / 2 ) n ] × v m − 1 / 2 n + g m − 1 / 2 n } \left\{ { - \left[ {2{\bf{\omega }}_{ie(m - 1/2)}^n + {\bf{\omega }}_{en(m - 1/2)}^n} \right] \times {\bf{v}}_{m - 1/2}^n + {\bf{g}}_{m - 1/2}^n} \right\} {−[2ωie(m−1/2)n​+ωen(m−1/2)n​]×vm−1/2n​+gm−1/2n​}eth.gcc = eth.gn - cross(eth.wnien,vn); % 考虑重力/哥氏力/向心力

2、位置更新

 eth.RMh = Re*(1-e2)/sq/sq2+pos(3);  % 子午圈主曲率半径+飞行高度   （3.1.23）
 eth.RNh = Re/sq2+pos(3);  eth.clRNh = eth.cl*eth.RNh;  % 卯酉圈主曲率半径+飞行高度   （3.1.24）
 nn = 2; ts = 0.1; nts = nn*ts; % nn -- 子样数；ts -- 采样时间间隔；nts -- 采样周期

% 位置更新
 vn = (vn+vn1)/2;
 pos = pos + [vn(2)/eth.RMh;vn(1)/eth.clRNh;vn(3)]*nts;  
 vn = vn1;

p m = p m − 1 + M p v ( m − 1 / 2 ) ( v m − 1 n ( m − 1 ) + v m n ( m ) ) T 2 {{\bf{p}}_m} = {{\bf{p}}_{m - 1}} + {{\bf{M}}_{pv(m - 1/2)}}({\bf{v}}_{m - 1}^{n(m - 1)} + {\bf{v}}_m^{n(m)})\frac{T}{2} pm​=pm−1​+Mpv(m−1/2)​(vm−1n(m−1)​+vmn(m)​)2T​

在这里插入图片描述

3、姿态更新

姿态更新求解方法是，先使用陀螺角增量的多子样采样计算等效旋转矢量，补偿转动不可交换误差，再使用等效旋转矢量计算姿态更新四元数。
选取“东–北–天（E–N–U）”地理坐标系作为捷联惯导系统的导航参考坐标系，后面记为n系，则以 系作为参考系的姿态微分方程为

 eth.wnin = eth.wnie + eth.wnen;  % 载体相对惯性系的角速度
 [phim, dvbm] = cnscl(wm, vm);  % 圆锥误差/划船误差补偿；phim -- 圆锥补偿后旋转矢量；dvbm -- 划桨补偿后速度增量dvbm = (vmm+0.5*cross(wmm,vmm)+scullm)';
 % 姿态更新
 qnb = qmul(rv2q(-eth.wnin*nts), qmul(qnb, rv2q(phim)));  
 qnb = qnormlz(qnb);

ω i n n {\bf{\omega }}_{in}^n ωinn​表示n系相对于i系的旋转，它包含两部分：地球自转引起的导航系旋转，以及惯导系统在地球表面附近移动因地球表面弯曲引起的n系旋转：
ω i n n = ω i e n + ω e n n {\bf{\omega }}_{in}^n = {\bf{\omega }}_{ie}^n + {\bf{\omega }}_{en}^n ωinn​=ωien​+ωenn​
C b ( m ) b ( m − 1 ) {\bf{C}}_{b(m)}^{b(m - 1)} Cb(m)b(m−1)​矩阵表示以i系作为参考基准，b系从m-1时刻到m时刻的旋转变化， C b ( m ) b ( m − 1 ) {\bf{C}}_{b(m)}^{b(m - 1)} Cb(m)b(m−1)​可由陀螺角速度 ω i b b {\bf{\omega }}_{ib}^b ωibb​确定； C n ( m − 1 ) n ( m ) {\bf{C}}_{n(m - 1)}^{n(m)} Cn(m−1)n(m)​表示以i系作为参考基准，n系从m-1时刻到m时刻的旋转变化， C n ( m − 1 ) n ( m ) {\bf{C}}_{n(m - 1)}^{n(m)} Cn(m−1)n(m)​可由计算角速度 − ω i n n - {\bf{\omega }}_{in}^n −ωinn​确定。
C b ( m ) n ( m ) = C n ( m − 1 ) n ( m ) C i n ( m − 1 ) C b ( m − 1 ) i C b ( m ) b ( m − 1 ) = C n ( m − 1 ) n ( m ) C b ( m − 1 ) n ( m − 1 ) C b ( m ) b ( m − 1 ) {\bf{C}}_{b(m)}^{n(m)} = {\bf{C}}_{n(m - 1)}^{n(m)}{\bf{C}}_i^{n(m - 1)}{\bf{C}}_{b(m - 1)}^i{\bf{C}}_{b(m)}^{b(m - 1)} = {\bf{C}}_{n(m - 1)}^{n(m)}{\bf{C}}_{b(m - 1)}^{n(m - 1)}{\bf{C}}_{b(m)}^{b(m - 1)} Cb(m)n(m)​=Cn(m−1)n(m)​Cin(m−1)​Cb(m−1)i​Cb(m)b(m−1)​=Cn(m−1)n(m)​Cb(m−1)n(m−1)​Cb(m)b(m−1)​
Q b ( m ) n ( m ) = Q n ( m − 1 ) n ( m ) Q i n ( m − 1 ) Q b ( m − 1 ) i Q b ( m ) b ( m − 1 ) = Q n ( m − 1 ) n ( m ) Q b ( m − 1 ) n ( m − 1 ) Q b ( m ) b ( m − 1 ) {\bf{Q}}_{b(m)}^{n(m)} = {\bf{Q}}_{n(m - 1)}^{n(m)}{\bf{Q}}_i^{n(m - 1)}{\bf{Q}}_{b(m - 1)}^i{\bf{Q}}_{b(m)}^{b(m - 1)} = {\bf{Q}}_{n(m - 1)}^{n(m)}{\bf{Q}}_{b(m - 1)}^{n(m - 1)}{\bf{Q}}_{b(m)}^{b(m - 1)} Qb(m)n(m)​=Qn(m−1)n(m)​Qin(m−1)​Qb(m−1)i​Qb(m)b(m−1)​=Qn(m−1)n(m)​Qb(m−1)n(m−1)​Qb(m)b(m−1)​
若陀螺在时间段 [ t m − 1 , t m ] [{t_{m - 1}},{t_m}] [tm−1​,tm​] 内（ T = t m − t m − 1 T = {t_m} - {t_{m - 1}} T=tm​−tm−1​）进行了两次等间隔采样，角增量分别为 Δ θ m 1 {\rm{\Delta }}{{\bf{\theta }}_{m1}} Δθm1​和 Δ θ m 2 {\rm{\Delta }}{{\bf{\theta }}_{m2}} Δθm2​ ，采用二子样圆锥误差补偿算法，有
C b ( m ) b ( m − 1 ) = M R V ( ϕ i b ( m ) b ) {\bf{C}}_{b(m)}^{b(m - 1)} = {{\bf{M}}_{{\rm{RV}}}}({\phi }_{ib(m)}^b) Cb(m)b(m−1)​=MRV​(ϕib(m)b​)
ϕ i b ( m ) b = ( Δ θ m 1 + Δ θ m 2 ) + 2 3 Δ θ m 1 × Δ θ m 2 {\phi }_{ib(m)}^b = ({\rm{\Delta }}{{\bf{\theta }}_{m1}} + {\rm{\Delta }}{{\bf{\theta }}_{m2}}) + \frac{2}{3}{\rm{\Delta }}{{\bf{\theta }}_{m1}} \times {\rm{\Delta }}{{\bf{\theta }}_{m2}} ϕib(m)b​=(Δθm1​+Δθm2​)+32​Δθm1​×Δθm2​
由速度和位置引起的 ω i n n {\bf{\omega }}_{in}^n ωinn​变化很小，即 ω i n n {\bf{\omega }}_{in}^n ωinn​可视为常值：
C n ( m − 1 ) n ( m ) = ( C n ( m ) n ( m − 1 ) ) T = M R V T ( ϕ i n ( m ) n ) ≈ M R V T ( T ω i n ( m ) n ) {\bf{C}}_{n(m - 1)}^{n(m)} = {({\bf{C}}_{n(m)}^{n(m - 1)})^{\rm{T}}} = {\bf{M}}_{{\rm{RV}}}^{\rm{T}}({\phi }_{in{\rm{(}}m)}^n) \approx {\bf{M}}_{RV}^{\rm{T}}(T{\bf{\omega }}_{in(m)}^n) Cn(m−1)n(m)​=(Cn(m)n(m−1)​)T=MRVT​(ϕin(m)n​)≈MRVT​(Tωin(m)n​)
= I + sin ⁡ ϕ ϕ ( ϕ × ) + 1 − cos ⁡ ϕ ϕ 2 ( ϕ × ) 2 = {\bf{I}} + \frac{{\sin \phi }}{\phi }(\boldsymbol{\phi } \times ) + \frac{{1 - \cos \phi }}{{{\phi ^2}}}{(\boldsymbol{\phi } \times )^2} =I+ϕsinϕ​(ϕ×)+ϕ21−cosϕ​(ϕ×)2

符号含义 / 程序中表现为
C n ( m − 1 ) n ( m ) {\bf{C}}_{n(m - 1)}^{n(m)} Cn(m−1)n(m)​rv2q(-eth.wnin*nts)
C b ( m ) b ( m − 1 ) {\bf{C}}_{b(m)}^{b(m - 1)} Cb(m)b(m−1)​rv2q(phim);phim = (wmm+dphim)';dphim = cross(csw,wm(n,:)); % 圆锥补偿量

4、程序仿真及实验结果

4.1 主函数

clear
clc
format long
gvar; %加载全局变量
nn=1;    %子样数，采用二样子圆锥误差补偿算法
ts=0.01;  %单个采样间隔的采样时间长度
nts=nn*ts; %采样周期，在nts时间内进行两次采样，每次采样 0.1秒
%输入姿态角向量att含三个分量，依次为俯仰角θ(Theta)，横滚角γ(Gama)，航向角ψ(Fai)
att = [0;0;90]*arcdeg; vn = [0;0;0]; pos = [[34;108]*arcdeg;100];
% att=[-1.42701;0.569073;-176.015]*arcdeg;    
% vn=[0;0;0];  %初始化速度
% %初始位置 纬度（B）34.8222007度 ,经度（L）113.4845389度 ，高度（h）87.2566m   B L H相当于大地坐标系的纬度、经度、高度
% pos=[34.8222007*arcdeg;113.4845389*arcdeg;87.2566]; 
qnb=a2qua(att);% 姿态、速度和位置初始化  ；姿态角转换为四元数  b->n
%仿真静态IMU数据
% eth=earth(pos,vn); %地球导航参数参数计算
% %qconj 四元数共轭，地球自转角速度由 导航系换算到机体系，作为机体角速度，并乘时间得到角度增量
% wm=qmulv(qconj(qnb),eth.wnie)*ts;  
% %比力由 导航系换算到机体系，作为机体比力，并乘时间得速度增量
% vm=qmulv(qconj(qnb),-eth.gn)*ts;   
% wm=repmat(wm',nn,1); %repmat 重复数组副本。转置角速度矢量，并复制到nn行'1'列 相当于二子样
% vm=repmat(vm',nn,1); %转置速度矢量，并复制得到nn行'1'列
%仿真时间   
phi=[0.1;0.2;3]*arcmin;  %由计算四元数和真实四元数计算失准角误差  phi
qnb=qaddphi(qnb,phi);    %四元数加失准角误差 转换为qnb           
len=fix(100/ts)-1;        %仿真时长，得到fix(1550/ts)个子样 fix(1550/ts)
avp=zeros(len,10);  %由零组成的len*10的向量
kk=1;  
t=0; 
load('imu.mat'); %加载imu数据  avp=[wm, vm, tt(2:end)]; gx(rad)  gy  gz ax (m/s)  ay  az time
trj=load ('trj.mat');                                %trj=[att, vn, pos]; att(度) vn(米每秒) pos(米)
avp1=zeros(fix(len/2+1),10);
avp1(1,:)=[att;vn;pos;0]';
%记录导航结果[att,vn,pos,t]  共有len个子样，每个采样时间段nn个子样，共len/nn个时间段，循环len/nn步                               
for k=1:nn:len-1                                  %k是两次一跳，因为采用的是二子样，36000是6分钟开始解算
    t=t+nts;                                        %t表示采样时间段
    
     wm=avp(k,1:3);
     vm=avp(k,4:6);
    
    [qnb,vn,pos]=insupdate(qnb,vn,pos,wm,vm,ts);
    kk=kk+1; 
    avp1(kk,:)=[q2att(qnb);vn;pos;t]';               %avp()=[θ γ ψ VE VN VU B L h t]
    if mod(t,1)<nts,  disp(fix(t));  
    end                                             %显示进度  disp函数：显示文本或数组
end

%取出avp中只有1s时的数据
x=fix(len/100);
avp0=zeros(x,10);
k1=1;
for i=1:length(avp1)
    c=mod(avp1(i,10),1);
    d=1-c;
    if(c<0.0000000001||d<=0.000001)
        avp0(k1,:)=avp1(i,:); %avp1()=[θ γ ψ VE VN VU B L h t*100]
        k1=k1+1;
    end
end

%姿态
tt=length(avp1);
tf=length(trj.trj);
figure(1);
subplot(211);
plot(1:tt,(avp1(1:tt,1:2)/arcdeg),1:tf,(trj.trj(1:tf,1:2)/arcdeg),'--'); title('俯仰角和横滚角');xlabel('d');ylabel('\theta,\gamma(\circ)');
legend('\it\theta','\it\gamma','\bf\theta','\bf\gamma') ;grid on;
subplot(212);
plot(1:tt,(avp1(1:tt,3)/arcdeg),1:tf,(trj.trj(1:tf,3)/arcdeg),'--'); title('偏航角');xlabel('d');ylabel('\Phi(\circ)');
legend('\it\Phi','\bf\Phi') ;grid on;
figure(2);
subplot(211);
plot(1:tt,(avp1(1:tt,4:5)/arcdeg),1:tf,(trj.trj(1:tf,4:5)/arcdeg),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_E','\itv\rm_N','\bfv\rm_E','\bfv\rm_N') 
subplot(212);grid on;
plot(1:tt,(avp1(1:tt,6)/arcdeg),1:tf,(trj.trj(1:tf,6)/arcdeg),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_U','\bfv\rm_U');grid on;
figure(3);
plot(1:tt,deltapos(avp1(1:tt,7:9)),1:tf,deltapos(trj.trj(1:tf,7:9)),'--'); title('位置');xlabel('d');ylabel('\DeltaPos / m');
legend('\Delta\itL', '\Delta\it\lambda', '\Delta\ith','\Delta\bfL', '\Delta\bf\lambda', '\Delta\bfh');grid on;

4.2 子函数

function [qnb, vn, pos, eth] = insupdate(qnb, vn, pos, wm, vm, ts)  % 捷联惯导数值更新算法
    nn = size(wm,1);  nts = nn*ts;
    [phim, dvbm] = cnscl(wm, vm);  % 圆锥误差/划船误差补偿；phim -- 补偿后旋转矢量；dvbm -- 补偿后速度增量
    eth = earth(pos, vn);  % 地球相关参数计算
    vn1 = vn + rv2m(-eth.wnin*nts/2)*qmulv(qnb,dvbm) + eth.gcc*nts;  % 速度更新  
    vn = (vn+vn1)/2;
    pos = pos + [vn(2)/eth.RMh;vn(1)/eth.clRNh;vn(3)]*nts;  vn = vn1;  % 位置更新
    qnb = qmul(rv2q(-eth.wnin*nts), qmul(qnb, rv2q(phim)));  % 姿态更新
    qnb = qnormlz(qnb);

function eth = earth(pos, vn)  % 地球相关参数计算
global Re ff wie g0
    ee = sqrt(2*ff-ff^2);  e2 = ee^2;  % 第一偏心率
    eth.sl = sin(pos(1));  eth.cl = cos(pos(1));  eth.tl = eth.sl/eth.cl;  % eth.sl -- sinL；eth.cl -- cosL；eth.tl -- tanL;
    eth.sl2 = eth.sl*eth.sl;  sl4 = eth.sl2*eth.sl2;  % eth.sl2 -- (sinL)^2；sl4 -- (sin2L)^2
    sq = 1-e2*eth.sl2;  sq2 = sqrt(sq);
    eth.RMh = Re*(1-e2)/sq/sq2+pos(3);  % 子午圈主曲率半径+飞行高度   （3.1.23）
    eth.RNh = Re/sq2+pos(3);  eth.clRNh = eth.cl*eth.RNh;  % 卯酉圈主曲率半径+飞行高度   （3.1.24）
    eth.wnie = wie*[0; eth.cl; eth.sl];  % 地球自转角速度三个分量值x y z
    eth.pos = pos; eth.vn = vn;   % 载体运行的速度等地球速度，作为已知量
    eth.wnen = [-vn(2)/eth.RMh; vn(1)/eth.RNh; vn(1)/eth.RNh*eth.tl];  % 载体运动的角速度分量，设定东“+”西“-”。 （3.1.24）
    eth.wnin = eth.wnie + eth.wnen;  % 载体相对惯性系的角速度
    eth.wnien = eth.wnie + eth.wnin;  % 由比力方程推导的公式，2wie+wen
    gLh = g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80重力模型 (3.2.23) g0=ge;考虑高度不变，不加pos(3)
    eth.gn = [0;0;-gLh];
    eth.gcc = eth.gn - cross(eth.wnien,vn); % 考虑重力/哥氏力/向心力

% %全局变量(gvar)
% global GM Re ff wie ge gp ug arcdeg arcmin arcsec hur dph dpsh ugpsHz  %定义全局变量
% GM=3.986004415e14;      %引力
% Re=6.378136998405e6;    %赤道长半轴
% wie= 7.2921151467e-5;   %自传角速度 wgs-84 model
% ff=1/298.257223563;     %扁率
% ee=sqrt(2*ff-ff^2);     %扁心率
% e2=ee^2;
% Rp=(1-ff)*Re;           %极轴半径
% ge=9.780325333434361;   %ge 赤道重力加速度
% gp=9.832184935381024;   %gp 极点重力加速度
% g0=9.780325333434361;   %重力加速度
% ug=ge*1e-6;            %ug 微重力加速度
% arcdeg=pi/180;         %弧度=角度×π÷180°
% arcmin=arcdeg/60;
% arcsec=arcmin/60;      %angle unit .acdeg为弧度
% hur=3600;
% dph=arcdeg/hur;
% dpsh=arcdeg/sqrt(hur); %hour;deg/hour;deg/sqrt(hour)
% ugpsHz= ug/sqrt(1);    %ug/sqrt(Hz)

4.3 实验结果

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

一、低成本MEMS惯导系统姿态、位置、速度更新算法

1、速度更新

忽略地球自转及地球曲率的影响，将速度更新方程简化为：

在这里插入图片描述
https://img-blog.csdnimg.cn/5f695ac22615448dbc594ff09659295e.png

arcdeg = pi/180;
imu=load ('imu.mat');                //imu数据： avp=[wm, vm, tt(2:end)]; gx(rad)  gy  gz ax (m/s)  ay  az time  
//imu输出速率信息:
//Tm=2*(imu(2,1)-imu(1,1));
//delta_theta1=imu(1,2:4)*Tm*arcdeg;delta_theta2=imu(2,2:4)*Tm*arcdeg;
//imu输出增量信息:
delta_theta_m=imu.avp(k,1:3);
delta_v_m=imu.avp(k,4:6);
pos = [34*arcdeg; 108*arcdeg; 100];  //位置初始化

//求gn:
g0=9.780325333434361;%单位;m/s^2
sl = sin(pos(1)); % pos(1)=L
sl2 = sl*sl;  sl4 = sl2*sl2;
gLh = g0*(1+5.27094e-3*sl2+2.32718e-5*sl4)-3.086e-6*pos(3); 
gn = [0;0;-gLh];

//速度更新:
vsf =q2mat(qnb)*(delta_v_m + 1/2*cross(delta_theta_m,delta_v_m))';     
vn2 = vn1 + vsf + gn*nts;             // %avp2(4:6) = avp1(4:6) + vsf + gn*Tm;

2、位置更新

在这里插入图片描述
不考虑单位换算：

%%%%%%%%1.按书上编写，未做修改，经度会产生剧烈震荡%%%%%%%%%%%%%%%%%
pos = pos+((vn1+vn2)/2)*nts;           //avp2(7:9)=avp1(7:9)+(avp1(4:6)+avp2(4:6))*Tm/2;

在这里插入图片描述
考虑单位换算，以及对误差进行补偿：

%%%%%%%%2.速度项乘以Mpv（4-1.58）%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
vn1 = (vn2+vn1)/2;
pos = pos + [vn1(2)/eth.RMh;vn1(1)/eth.clRNh;vn1(3)]*nts;

在这里插入图片描述
震荡原因与地球自转无关，是速度引起的经纬度变化：因为必须将速度项的单位化为弧度每秒，然后乘以时间得到与经纬度一致的弧度单位才能相加！！！！！
在这里插入图片描述

在这里插入图片描述

3、姿态更新

在这里插入图片描述

%%%%%%%%1.自己编写的没有加入地球自转所产生的误差补偿，经度位置增量偏小%%
q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
q_nb2 = qmul(qnb,q_bb); 
qnb = qnormlz(q_nb2);

%%%%%%%%2.在上一次的基础上加入地球自转补偿，即q_nn项%%%%%%%%%%%%%%%%%%
q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
qnb = qmul(rv2q(-eth.wnin*nts), qmul(qnb, q_bb));  % 姿态更新
qnb = qnormlz(qnb);

4、程序仿真及实验结果

4.1 主函数

clc
clear            
imu=load ('imu.mat');                                %imu数据： avp=[wm, vm, tt(2:end)]; gx(rad)  gy  gz ax (m/s)  ay  az time  
trj=load ('trj.mat');                                %trj=[att, vn, pos]; att(度) vn(米每秒) pos(米)
arcdeg = pi/180;   
nn=1;                                                %子样数，采用二样子圆锥误差补偿算法
ts=0.01;                                             %单个采样间隔的采样时间长度
nts=nn*ts;                                           %采样周期，在nts时间内进行两次采样，每次采样 0.1秒
att = [0;0;90]*arcdeg; vn = [0;0;0]; pos = [[34;108]*arcdeg;100]; % att -- 欧拉角；arcdeg -- 度转换为弧度；vn -- 速度；pos -- 位置
% eth=earth(pos,vn);
% x=eth.clRNh*cos(pos(2));
% y=eth.clRNh*sin(pos(2));
% z=eth.RNeh*sin(pos(1));
% pos=[x;y;z];
qnb = a2qua(att);  
len=length(imu.avp);
avps=zeros(fix(len/2),10);
avps(1,:)=[att;vn;pos;0]';
t=0;kk=1;
for k=1:nn:len                                       %k是两次一跳，因为采用的是二子样，36000是6分钟开始解算
    t=t+nts;
    delta_theta_m=imu.avp(k,1:3);
    delta_v_m=imu.avp(k,4:6);
    [qnb,vn,pos]=insupdate(qnb,vn,pos,delta_theta_m,delta_v_m,ts);
    cnb=q2att(qnb);
    cnb(3)=mod(cnb(3),2*pi);    
    kk=kk+1; 
    avps(kk,:)=[cnb; vn; pos; t]';           %avp()=[θ γ ψ VE VN VU B L h t]
    if mod(t,1)<nts,  disp(fix(t));  
    end                                             %显示进度  disp函数：显示文本或数组
end
    
%姿态
tt=length(avps);
tf=length(trj.trj);
figure(1);
subplot(211);
plot(1:tt,(avps(1:tt,1:2)/arcdeg),1:tf,(trj.trj(1:tf,1:2)/arcdeg),'--'); title('俯仰角和横滚角');xlabel('d');ylabel('\theta,\gamma(\circ)');
legend('\it\theta','\it\gamma','\bf\theta','\bf\gamma') ;grid on;
subplot(212);
plot(1:tt,(avps(1:tt,3)/arcdeg),1:tf,(trj.trj(1:tf,3)/arcdeg),'--'); title('偏航角');xlabel('d');ylabel('\Phi(\circ)');
legend('\it\Phi','\bf\Phi') ;grid on;
%速度
figure(2);
subplot(211);
plot(1:tt,(avps(1:tt,4:5)/arcdeg),1:tf,(trj.trj(1:tf,4:5)/arcdeg),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_E','\itv\rm_N','\bfv\rm_E','\bfv\rm_N') 
subplot(212);grid on;
plot(1:tt,(avps(1:tt,6)/arcdeg),1:tf,(trj.trj(1:tf,6)/arcdeg),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_U','\bfv\rm_U');grid on;
%位置
figure(3);
plot(1:tt,deltapos(avps(1:tt,7:9)),1:tf,deltapos(trj.trj(1:tf,7:9)),'--'); title('位置');xlabel('d');ylabel('\DeltaPos / m');
legend('\Delta\itL', '\Delta\it\lambda', '\Delta\ith','\Delta\bfL', '\Delta\bf\lambda', '\Delta\bfh');grid on;

4.2 子函数

function [qnb,vn2,pos]=insupdate(qnb,vn1,pos,delta_theta_m,delta_v_m,ts)
nn=1;                                                                  %子样数，采用二样子算法   
nts=nn*ts;
g0=9.780325333434361;                                                  %单位;m/s^2
sl = sin(pos(1));                                                      % pos(1)=L
sl2 = sl*sl;  sl4 = sl2*sl2;
gLh = g0*(1+5.27094e-3*sl2+2.32718e-5*sl4)-3.086e-6*pos(3); 
gn = [0;0;-gLh];
eth = earth(pos, vn1);  % 地球相关参数计算
%速度更新
vsf =q2mat(qnb)*(delta_v_m + 1/2*cross(delta_theta_m,delta_v_m))';      %avp2(4:6) = avp1(4:6) + vsf + gn*Tm;
vn2 = vn1 + vsf + gn*nts;
%位置更新
%%%%%%%%2.速度项乘以Mpv（4-1.58）%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
vn1 = (vn2+vn1)/2;
pos = pos + [vn1(2)/eth.RMh;vn1(1)/eth.clRNh;vn1(3)]*nts;
%%%%%%%%1.按书上编写，未做修改，经度会产生剧烈震荡%%%%%%%%%%%%%%%%%%%%
% pos = pos+((vn1+vn2)/2)*nts;                                           %avp2(7:9)=avp1(7:9)+(avp1(4:6)+avp2(4:6))*Tm/2;
%姿态更新                                                                % q_nb1=att2qua(avp1(1:3));%姿态角转化为四元数
%%%%%%%%2.在上一次的基础上加入地球自转补偿，即q_nn项%%%%%%%%%%%%%%%%%%
q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
qnb = qmul(rv2q(-eth.wnin*nts), qmul(qnb, q_bb));  % 姿态更新
qnb = qnormlz(qnb);
%%%%%%%%1.自己编写的没有加入地球自转所产生的误差补偿，经度位置增量偏小%%
% q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
% q_nb2 = qmul(qnb,q_bb); 
% qnb = qnormlz(q_nb2);
end

4.3 实验结果

1. 不考虑地球自转的影响

在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

在这里插入图片描述
在这里插入图片描述

2. 考虑地球自转的影响

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

5. 程序代码及数据下载地址

https://download.csdn.net/download/qq_38364548/87364983

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/142009.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

乾元浩在创业板IPO终止：主要生产禽用疫苗产品，中农大是股东

乾元浩在创业板IPO终止：主要生产禽用疫苗产品，中农大是股东

2023年1月4日&#xff0c;深圳证券交易所披露的信息显示&#xff0c;乾元浩生物股份有限公司&#xff08;下称“乾元浩”&#xff09;提交了撤回上市申请文件的申请&#xff0c;保荐人中信证券也撤回对该公司的保荐。因此&#xff0c;深交所终止了乾元浩首次公开发行股票并在创…
阅读更多...
程序员述职报告

程序员述职报告

程序员述职报告笔者能力有限&#xff0c;仅供参考。做研发的小伙伴&#xff0c;不太擅长于写PPT&#xff0c;对于更高一级别的领导&#xff0c;可能只有年终述职的时候才能全面的了解你的工作。所以需要我们在PPT中表达自我价值&#xff0c;突出角色职责。让领导更清楚的认识你…
阅读更多...
访问数据库超时问题排障

访问数据库超时问题排障

1 排障过程 系统从圣诞节那天晚上开始&#xff0c;每天晚上固定十点多到十一点多这个时段&#xff0c;大概瘫痪1h左右&#xff0c;过这时段系统自动恢复。系统瘫痪时的现象就是&#xff0c;网页和App都打不开&#xff0c;请求超时。系统架构&#xff1a; 整个系统托管在公有云…
阅读更多...
图解数据结构：盘点链表与栈和队列的那些血缘（单双链表模拟实现栈和队列）

图解数据结构：盘点链表与栈和队列的那些血缘（单双链表模拟实现栈和队列）

写在前面 Hello&#xff0c;各位盆友们&#xff0c;我是黄小黄。关于前一段时间为什么拖更这件事&#xff0c;这里给大家说一句抱歉。笔者前段时间忙于ddl和一些比赛相关的事件&#xff0c;当然还有些隐藏任务&#xff0c;所以博文更新就放缓了。  这里还需要做一下对以后博文…
阅读更多...
计算机原理一_计算机的组成、进程与线程

计算机原理一_计算机的组成、进程与线程

目录儿一、计算机组成二、进程与线程2.1 线程的切换2.2 CPU的并发控制2.2.1 关中断2.2.2 缓存一致性协议2.2.2.1 缓存Cache2.2.2.2 缓存行Cache Line2.2.2.3 缓存一致性拓展&#xff1a;超线程2.2.3 内存屏障2.2.3.1 CPU的乱序执行拓展1&#xff1a;java 的 this 溢出问题拓展2…
阅读更多...
Linux（一）：Linux基本结构

Linux（一）：Linux基本结构

一、Linux系统划分 linux系统分为用户区、内核区 1.1分区目标 保护数据和硬件安全&#xff0c;对系统进行分区也就是进程分区&#xff0c;当处于用户态&#xff0c;只能访问用户区&#xff0c;用户无法修改内核&#xff0c;保证硬件安全&#xff0c;操作系统不易损坏&#…
阅读更多...
【Qt】QMainWindow应用程序窗口类简单介绍

【Qt】QMainWindow应用程序窗口类简单介绍

QMainWindow介绍 QMainWindow是一个为用户提供主窗口程序的类&#xff0c;是许多应用程序的基础&#xff0c;包含的组件有&#xff1a; 菜单栏QMenuBar&#xff0c;一个主窗口最多只能有一个菜单栏&#xff1b;包含一个下拉菜单项的列表&#xff0c;这些菜单项由QAction动作类…
阅读更多...
【git版本控制】| git版本控制操作命令（全）

【git版本控制】| git版本控制操作命令（全）

文章目录一、简介二、工作模式1 集中式&#xff08;CVS、SVN&#xff09;2 分布式Git三、Git1 工作模式2 git工作流程3 工作区和版本库4 注意事项5 基本操作5.1 创建本地版本库5.2 初始化本地版本库5.3 .git目录的作用5.4 创建用户5.5 其他操作6 git分支7 常见警告8 免密登录9 …
阅读更多...
interface接口--GO面向对象编程思想

interface接口--GO面向对象编程思想

一、interface接口 interface 是GO语言的基础特性之一。可以理解为一种类型的规范或者约定。它跟java&#xff0c;C# 不太一样&#xff0c;不需要显示说明实现了某个接口&#xff0c;它没有继承或子类或“implements”关键字&#xff0c;只是通过约定的形式&#xff0c;隐式的…
阅读更多...
【C语言进阶】自定义类型：结构体，枚举，联合体

【C语言进阶】自定义类型：结构体，枚举，联合体

目录 1、结构体的声明 1.1 结构体基础知识 1.2 结构体的声明 1.3 特殊的声明 1.4 结构体的自引用 1.5 结构体变量的定义和初始化 1.6 结构体内存对齐 ​编辑1.7 修改默认对齐数 1.8 结构体传参 2. 位段 2.1 什么是位段 2.2 位段的内存分配 2.3 位段的跨平台问…
阅读更多...
【owt-server】代码结构及新增一个agent

【owt-server】代码结构及新增一个agent

owt server 官方 5.0 仓库：代码结构 manage console manage api portal sip portal 与agent 并列 agent又有很多种类。 启动脚本 启动一个新的agent 比如streaming-agent streaming-agent )cd ${OWT_HOME}/s
阅读更多...
分布式id

分布式id

分布式id一 什么是分布式系统唯一ID二 分布式系统唯一ID的特点三 分布式系统唯一ID的实现方案3.1 基于UUID3.2 基于数据库自增id3.3 基于数据库集群模式3.4 基于Redis模式3.5 基于雪花算法&#xff08;Snowflake&#xff09;模式3.6 百度&#xff08;uid-generator&#xff09;…
阅读更多...
Python爬虫数据到sqlite实例

Python爬虫数据到sqlite实例

参考链接:https://blog.csdn.net/qq_45775027/article/details/115319253最近需要使用到爬虫数据库&#xff0c;原文中作者有些没补齐&#xff0c;略作修改之后跑通了。主要修改&#xff1a;1.调整了数据获取的正则表达式&#xff1b;2. 改了一下数据库的table名和定义名字&…
阅读更多...
基于Java+SpringBoot+vue+element实现前后端分离牙科诊所管理系统详细设计

基于Java+SpringBoot+vue+element实现前后端分离牙科诊所管理系统详细设计

基于JavaSpringBootvueelement实现前后端分离牙科诊所管理系统详细设计 博主介绍&#xff1a;5年java开发经验&#xff0c;专注Java开发、定制、远程、文档编写指导等,csdn特邀作者、专注于Java技术领域 作者主页 超级帅帅吴 欢迎点赞 收藏 ⭐留言 文末获取源码联系方式 文章目…
阅读更多...
【Linux】虚拟地址空间 --- 虚拟地址、空间布局、内存描述符、写时拷贝、页表…

【Linux】虚拟地址空间 --- 虚拟地址、空间布局、内存描述符、写时拷贝、页表…

该吃吃&#xff0c;该喝喝&#xff0c;遇事儿别往心上隔&#x1f60e; 文章目录一、虚拟地址空间1.虚拟地址的引出&#xff08;看不到物理地址&#xff0c;只能看看虚拟地址喽&#xff09;2.虚拟地址空间布局&#xff08;五个段&#xff09;3.感性理解一下虚拟地址空间&#xf…
阅读更多...
【C++修炼之路】C++入门（上）

【C++修炼之路】C++入门（上）

&#x1f451;作者主页&#xff1a;进击的安度因 &#x1f3e0;学习社区&#xff1a;进击的安度因&#xff08;个人社区&#xff09; &#x1f4d6;专栏链接&#xff1a;C修炼之路 文章目录一、前言二、第一个 C 程序三、C 关键字(C98)四、命名空间1、命名空间的定义2、命名空间…
阅读更多...
C++ Prime课后习题第一章编程

C++ Prime课后习题第一章编程

编程一个C程序&#xff0c;它显示您的姓名和地址。#include <iostream>int stonetolb(int); int main() {using namespace std;cout << "zzz ";cout << "闵行"<<endl;return 0; }编写一个程序&#xff0c;要求用户输入一个以long…
阅读更多...
3台机器配置hadoop集群_Hadoop+Hbase 分布式集群架构

3台机器配置hadoop集群_Hadoop+Hbase 分布式集群架构

安装搭建Hadoop1、 配置说明本次集群搭建共三台机器&#xff0c;具体说明下&#xff1a;主机名IP说明nn01192.168.1.51DataNode、NodeManager、ResourceManager、NameNodedn01192.168.1.52DataNode、NodeManager、SecondaryNameNodedn02192.168.1.53DataNode、NodeManager2 、安…
阅读更多...
基于浏览器的 PDF 编辑器:RAD PDF for ASP.NET

基于浏览器的 PDF 编辑器:RAD PDF for ASP.NET

版本 3.34 改进的 PDF 收藏/投资组合支持和服务器 API 改进 Ω578867473功能更新 为更基本的 PDF 文件损坏/语法错误添加了更正添加了 PdfButtonField.NamedAction 属性添加了 PdfButtonField.IsNamedAction 属性添加 PdfButtonField() 构造函数 - PdfButtonFields 可以由服…
阅读更多...
unity-常用组件实操案例

unity-常用组件实操案例

文章目录transform摄像机cameraskybox相机权重&#xff08;depth&#xff09;Audio sourcevideo playertransform 不但控制着组件的旋转、位置、缩放并且还控制着组件间的父子关系 using System; using System.Collections; using System.Collections.Generic; using UnityEn…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023