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

@曾记否

已于 2023-01-05 18:54:22 修改

阅读量2.5k

点赞数 8

分类专栏：卡尔曼滤波文章标签：算法

于 2023-01-05 15:41:34 首次发布

本文链接：https://blog.csdn.net/qq_38364548/article/details/128453174

版权

卡尔曼滤波专栏收录该内容

13 篇文章 71 订阅

订阅专栏

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

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

一、高成本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)

${\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$
$\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)})$

$\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$

符号	含义 / 程序中表现为
$\Delta {\bf{v}}_{{\rm{sf}}(m)}^n$	导航系比力速度增量
$\Delta {\bf{v}}_{{\rm{cor/g}}(m)}^n$	有害加速度的速度增量（4.1-27）
$\Delta {\bf{v}}_{{\rm{rot}}(m)}^{b(m - 1)}$	速度的旋转误差补偿量
$\Delta {\bf{v}}_{{\rm{scul}}(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)})$	dvbm – 补偿后速度增量`dvbm = (vmm+0.5*cross(wmm,vmm)+scullm)';`
$\left[ {{\bf{I}} - \frac{T}{2}({\bf{\omega }}_{in(m - 1/2)}^n \times )} \right]$	`rv2m(-eth.wnin*nts/2)`
$\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\}$	`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;

${{\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}$

在这里插入图片描述

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);

${\bf{\omega }}_{in}^n$ 表示n系相对于i系的旋转，它包含两部分：地球自转引起的导航系旋转，以及惯导系统在地球表面附近移动因地球表面弯曲引起的n系旋转：
${\bf{\omega }}_{in}^n = {\bf{\omega }}_{ie}^n + {\bf{\omega }}_{en}^n$
${\bf{C}}_{b(m)}^{b(m - 1)}$ 矩阵表示以i系作为参考基准，b系从m-1时刻到m时刻的旋转变化， ${\bf{C}}_{b(m)}^{b(m - 1)}$ 可由陀螺角速度 ${\bf{\omega }}_{ib}^b$ 确定； ${\bf{C}}_{n(m - 1)}^{n(m)}$ 表示以i系作为参考基准，n系从m-1时刻到m时刻的旋转变化， ${\bf{C}}_{n(m - 1)}^{n(m)}$ 可由计算角速度 ${\bf{\omega }}_{in}^n$ 确定。
${\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)}$
${\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)}$
若陀螺在时间段 ${t_{m - 1}},{t_m}]$ 内（ $T = {t_m} - {t_{m - 1}}$ ）进行了两次等间隔采样，角增量分别为 ${\rm{\Delta }}{{\bf{\theta }}_{m1}}$ 和 ${\rm{\Delta }}{{\bf{\theta }}_{m2}}$ ，采用二子样圆锥误差补偿算法，有
${\bf{C}}_{b(m)}^{b(m - 1)} = {{\bf{M}}_{{\rm{RV}}}}({\phi }_{ib(m)}^b)$
${\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}}$
由速度和位置引起的 ${\bf{\omega }}_{in}^n$ 变化很小，即 ${\bf{\omega }}_{in}^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)$
${\bf{I}} + \frac{{\sin \phi }}{\phi }(\boldsymbol{\phi } \times ) + \frac{{1 - \cos \phi }}{{{\phi ^2}}}{(\boldsymbol{\phi } \times )^2}$

符号	含义 / 程序中表现为
${\bf{C}}_{n(m - 1)}^{n(m)}$	`rv2q(-eth.wnin*nts)`
${\bf{C}}_{b(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、速度更新

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

在这里插入图片描述

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

@曾记否

关注

8
点赞
踩
38

收藏

觉得还不错? 一键收藏
0
评论
高、低成本MEMS惯导系统姿态、位置、速度更新算法的对比

姿态更新求解方法是，先使用陀螺角增量的多子样采样计算等效旋转矢量，补偿转动不可交换误差，再使用等效旋转矢量计算姿态更新四元数。选取“东–北–天（E–N–U）”地理坐标系作为捷联惯导系统的导航参考坐标系，后面记为n系，则以系作为参考系的姿态微分方程为。矩阵表示以i系作为参考基准，b系从m-1时刻到m时刻的旋转变化，表示以i系作为参考基准，n系从m-1时刻到m时刻的旋转变化，）进行了两次等间隔采样，角增量分别为。，以及惯导系统在地球表面附近移动。
复制链接

扫一扫

专栏目录