捷聯(lián)慣導(dǎo)算法與組合導(dǎo)航原理kalman濾波

1卡爾曼濾波與組合導(dǎo)航原理

（捷聯(lián)慣導(dǎo)算法部分）西北工業(yè)大學(xué)自動化學(xué)院嚴恭敏2016-112主要參考書目：《卡爾曼濾波與組合導(dǎo)航原理》，秦永元，西北工業(yè)大學(xué)出版社，2012《Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用》，付夢印，科學(xué)出版社，2010《慣性儀器測試與數(shù)據(jù)分析》，嚴恭敏，國防工業(yè)出版社，2012《捷聯(lián)慣導(dǎo)算法與組合導(dǎo)航原理》講義，嚴恭敏，2016先修課程：隨機過程；數(shù)理統(tǒng)計；矩陣論；自控原理；數(shù)字信號處理；捷聯(lián)慣導(dǎo)系統(tǒng)原理二、地球形狀與重力場31、地球形狀

旋轉(zhuǎn)軸的南端點和北端點分別稱為南極（點）和北極（點）；包含南北極點（即旋轉(zhuǎn)軸）的平面稱為子午面；子午面與旋轉(zhuǎn)橢球面的交線稱為子午圈（或經(jīng)圈）。通過格林尼治的經(jīng)線稱為本初子午線，或零度經(jīng)線。任一經(jīng)線所在子午面與本初子午線所在子午面之間的夾角，定義為經(jīng)度。

包含橢球中心且垂直于旋轉(zhuǎn)軸的平面稱為赤道面；赤道面與旋轉(zhuǎn)橢球面的交線稱為赤道；平行于赤道面的平面與橢球面的交線稱為緯圈。包含橢球面上某點法線的平面稱為法截面；法截面與橢球的交線稱為法截線；當法截面包含南北極點時，法截線即為子午圈；當法截面垂直于子午面時，法截線稱為卯酉圈。橢球面上一點與地心連線赤道面的夾角稱為地心緯度；該點橢圓法線與赤道面的夾角，也稱為測地緯度，地圖繪制和位置的確定一般都是基于測地緯度的，常稱之為地理緯度。圓球、三軸橢球與旋轉(zhuǎn)橢球。二、地球形狀與重力場41、地球形狀子午面橢圓相關(guān)計算橢圓方程扁率偏心率第二偏心率代入橢圓二、地球形狀與重力場51、地球形狀曲率半徑曲率公式子午圈主曲率半徑卯酉圈主曲率半徑二、地球形狀與重力場61、地球形狀地球系至地理系的旋轉(zhuǎn)二、地球形狀與重力場71、地球形狀地球系至地理系的旋轉(zhuǎn)二、地球形狀與重力場81、地球形狀速度與經(jīng)緯度變化率關(guān)系二、地球形狀與重力場91、地球形狀大地坐標與地心直角坐標之間轉(zhuǎn)換二、地球形狀與重力場102、重力場引力離心力(圓球假設(shè))重力圓球假設(shè)誤差大！二、地球形狀與重力場112、重力場

假設(shè)旋轉(zhuǎn)橢球表面上的重力方向處處垂直表面，即旋轉(zhuǎn)橢球表面為重力等位面（質(zhì)點沿表面運動不做功），1929年意大利人索密里安（Somigliana）給出公式克萊羅定理（法國,1743）重力扁率二、地球形狀與重力場122、重力場二、地球形狀與重力場132、重力場二、地球形狀與重力場142、重力場重力與高度的近似關(guān)系二、地球形狀與重力場152、重力場h<0時摘自：二、地球形狀與重力場162、重力場真垂線與地理垂線（Heiskanen公式,1967年）二、地球形狀與重力場172、重力場(重力異常)二、地球形狀與重力場182、重力場(重力異常)位函數(shù)函數(shù)球諧函數(shù)二、地球形狀與重力場192、重力場(重力異常)2190階引力模型EIGEN-6C4，垂線偏差（選150階、經(jīng)度109°、高度0m）三、捷聯(lián)慣導(dǎo)更新算法201、姿態(tài)更新（東-北-天導(dǎo)航參考坐標系n）不用該思路！鏈乘拆解法三、捷聯(lián)慣導(dǎo)更新算法212、速度更新（比力方程）三、捷聯(lián)慣導(dǎo)更新算法222、速度更新（比力方程離散化）三、捷聯(lián)慣導(dǎo)更新算法232、速度更新（導(dǎo)航系旋轉(zhuǎn)修正）分部梯形三、捷聯(lián)慣導(dǎo)更新算法242、速度更新三、捷聯(lián)慣導(dǎo)更新算法252、速度更新（多項式擬合補償）劃船誤差二子樣補償算法三、捷聯(lián)慣導(dǎo)更新算法262、速度更新（劃船誤差補償）Matlab動畫演示三、捷聯(lián)慣導(dǎo)更新算法273、位置更新三、捷聯(lián)慣導(dǎo)更新算法284、仿真Matlab程序function[qnb,vn,pos,eth]=insupdate(qnb,vn,pos,wm,vm,ts)

nn=size(wm,1);nts=nn*ts;[phim,dvbm]=cnscl(wm,vm);%圓錐誤差/劃船誤差補償eth=earth(pos,vn);%地球相關(guān)參數(shù)計算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)));%姿態(tài)更新qnb=qnormlz(qnb);functioneth=earth(pos,vn)%地球相關(guān)參數(shù)計算globalReffwieg0ee=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.sl2=eth.sl*eth.sl;sl4=eth.sl2*eth.sl2;sq=1-e2*eth.sl2;sq2=sqrt(sq);eth.RMh=Re*(1-e2)/sq/sq2+pos(3);eth.RNh=Re/sq2+pos(3);eth.clRNh=eth.cl*eth.RNh;eth.wnie=wie*[0;eth.cl;eth.sl];eth.vn=vn;eth.wnen=[-vn(2)/eth.RMh;vn(1)/eth.RNh;vn(1)/eth.RNh*eth.tl];eth.wnin=eth.wnie+eth.wnen;eth.wnien=eth.wnie+eth.wnin;gLh=g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3);

eth.gn=[0;0;-gLh];eth.gcc=eth.gn-cross(eth.wnien,vn);%考慮重力/哥氏力/向心力function[phim,dvbm]=cnscl(wm,vm)cs=[[2,0,0,0,0]/3[9,27,0,0,0]/20[54,92,214,0,0]/105[250,525,650,1375,0]/504[2315,4558,7296,7834,15797]/4620];%補償系數(shù)wmm=sum(wm,1);vmm=sum(vm,1);

dphim=zeros(1,3);scullm=zeros(1,3);n=size(wm,1);%子樣數(shù)ifn>1csw=cs(n-1,1:n-1)*wm(1:n-1,:);csv=cs(n-1,1:n-1)*vm(1:n-1,:);dphim=cross(csw,wm(n,:));%圓錐補償scullm=cross(csw,vm(n,:))+cross(csv,wm(n,:));%劃船補償endphim=(wmm+dphim)';dvbm=(vmm+0.5*cross(wmm,vmm)+scullm)';四、捷聯(lián)慣導(dǎo)誤差模型291、慣性組件測量誤差模型陀螺誤差模型加計誤差模型四、捷聯(lián)慣導(dǎo)誤差模型302、姿態(tài)誤差方程計算導(dǎo)航坐標系四、捷聯(lián)慣導(dǎo)誤差模型313、速度誤差方程比力方程的理論公式實際計算公式求誤差四、捷聯(lián)慣導(dǎo)誤差模型324、位置誤差方程5、捷聯(lián)慣導(dǎo)誤差方程總結(jié)四、捷聯(lián)慣導(dǎo)誤差模型336、靜基座誤差特性高度通道水平通道四、捷聯(lián)慣導(dǎo)誤差模型346、靜基座誤差特性休拉頻率傅科頻率四、捷聯(lián)慣導(dǎo)誤差模型6、靜基座誤差特性glvsL=30*arcdeg;wN=wie*cos(L);wU=wie*sin(L);tL=tan(L);eL=sec(L);Ts=10;T=24*hur;t=(0:Ts:T-Ts)'/3600;len=length(t);Ut=[-[0.0;0.0;0.0]*dph;[100;00]*ug;0;0];X=zeros(7,len);X(:,1)=[[0;0;0]*arcmin;[0;0];[0;0]/Re];Ft=[0wU-wN0-1/Re00-wU001/Re0-wU0wN00tL/Re0wN00-g0002*wU00g000-2*wU00000001/Re00000eL/Re000];[Fk,Bk]=c2d(Ft,eye(size(Ft)),Ts);Uk=Bk*Ut;%離散化fork=2:length(t)X(:,k)=Fk*X(:,k-1)+Uk;%X([4:7],k)=0;%純失準角%X([1,3,5,6,7],k)=0;%東向通道%X([2,3,4,6,7],k)=0;%北向通道%X([3],k)=0;%水平通道%X([2,4],k)=0;%北向和方位通道endmysubplot(311,t,X(1:3,:)/arcmin,'t/hur','\phi_E,\phi_N\phi_U/\prime');mysubplot(312,t,X(4:5,:),'t/hur','\deltav_E,\deltav_N/m/s');mysubplot(313,t,X(6:7,:)/arcmin,'t/hur','\deltaL,\delta\lambda/\prime');東向陀螺常值漂移引起的誤差東向加速度計常值漂移引起的誤差五、捷聯(lián)慣導(dǎo)初始對準361、矢量定姿（雙矢量）常用算法：五、捷聯(lián)慣導(dǎo)初始對準371、矢量