航位推算在導(dǎo)航定位中的應(yīng)用

1、航位推算在導(dǎo)航定位中的應(yīng)用摘要：只限于GPS導(dǎo)航往往會(huì)有盲點(diǎn)，DR導(dǎo)航的自主性恰能彌補(bǔ)GPS導(dǎo)航的不足。本文闡述了在得到一組GPS導(dǎo)航數(shù)據(jù)的基礎(chǔ)上如何利用航位推算進(jìn)行導(dǎo)航。通過(guò)卡爾曼濾波對(duì)航向角數(shù)據(jù)進(jìn)行處理，比較濾波前后載體的運(yùn)動(dòng)軌跡，分析產(chǎn)生誤差的主要原因。結(jié)果表明：matlab仿真出的載體運(yùn)動(dòng)軌跡與實(shí)驗(yàn)時(shí)的羅盤(pán)方向軌跡相同，利用卡爾曼濾波能夠有效減小誤差。1 引言DR導(dǎo)航時(shí)一種自主式導(dǎo)航，成本相對(duì)GPS較低，在短時(shí)間內(nèi)能夠提供比較精確的導(dǎo)航參數(shù)，抗干擾能力強(qiáng)，隱蔽性好。但是誤差隨導(dǎo)航時(shí)間積累是DR導(dǎo)航的致命缺點(diǎn)，因此DR導(dǎo)航不適合長(zhǎng)時(shí)間的單獨(dú)導(dǎo)航。GPS導(dǎo)航精度高、功能強(qiáng)、使用方便，但在應(yīng)

2、用于車(chē)輛導(dǎo)航時(shí)，當(dāng)車(chē)輛在樹(shù)蔭下、涵洞、隧道里、深山峽谷內(nèi)，特別在城市里行駛時(shí)，高樓大廈會(huì)擋住衛(wèi)星的信號(hào)，使衛(wèi)星導(dǎo)航接收機(jī)不能正常接收衛(wèi)星信號(hào)，不能正常定位1。由此可見(jiàn)，將兩種導(dǎo)航方式結(jié)合起來(lái)共同完成導(dǎo)航功能是提高導(dǎo)航精度的一個(gè)主要手段，具有重大意義?？柭鼮V波在導(dǎo)航方面有很重要的作用，最傳統(tǒng)的導(dǎo)航融合是集中式Kalman濾波，現(xiàn)在應(yīng)用比較廣泛的是自適應(yīng)擴(kuò)展卡爾曼濾波，利用系統(tǒng)預(yù)測(cè)量、系統(tǒng)狀態(tài)噪聲量、觀測(cè)量與觀測(cè)噪聲幾個(gè)量實(shí)現(xiàn)濾波，在信息融合中，利用信息分配原理，自適應(yīng)調(diào)整GPS和DR觀測(cè)量在濾波中的權(quán)重。自適應(yīng)濾波根據(jù)自適應(yīng)因子作用范圍不同分為單因子自適應(yīng)濾波和多因子自適應(yīng)濾波2。圖1 航位推

3、算原理2 航位推算原理航位推算原理是利用載體的速度、航向以及上一時(shí)刻的位置來(lái)估計(jì)下一時(shí)刻載體的位置。利用載體的速度和航向能夠得出速度在當(dāng)?shù)厮阶鴺?biāo)軸上分向速度，將分向速度和載體所經(jīng)過(guò)的時(shí)間相乘便可以得到載體在坐標(biāo)軸上增加的坐標(biāo)值，與前一時(shí)刻的坐標(biāo)值求和便可以得到此時(shí)的坐標(biāo)值。原理圖如圖1所示。推算如公式(1)。.(1)其中，()為載體在時(shí)刻的初始位置，()為載體在時(shí)刻位置，、為載體在時(shí)刻的速度與航向角（航向角為載體運(yùn)動(dòng)方向與正東方向的夾角）。為羅盤(pán)相鄰兩次推算所用的時(shí)間差。3 坐標(biāo)系之間的轉(zhuǎn)化3.1 所涉坐標(biāo)系的種類(lèi)3.1.1 當(dāng)?shù)厮阶鴺?biāo)系當(dāng)?shù)厮阶鴺?biāo)系又名東北天坐標(biāo)系，它的坐標(biāo)原點(diǎn)為載體的

4、質(zhì)心，X軸指向當(dāng)?shù)乇弊游缇€，Y軸垂直X軸指向東，Z軸垂直XOY面構(gòu)成右手直角坐標(biāo)系。3.1.2大地坐標(biāo)系地球橢球的中心與地球質(zhì)心重合，橢球的短軸與地球的自轉(zhuǎn)軸重合，大地緯度B為地面點(diǎn)的橢球法線與橢球赤道面的夾角，大地經(jīng)度L為過(guò)地面點(diǎn)的橢球子午面與格林威治起始子午面的夾角，大地高H為地面點(diǎn)沿橢球法線至橢球面的距離。地面一點(diǎn)可表示為( L,B,H)。如圖3.1所示。圖3.1 大地坐標(biāo)系B:大地緯度，通過(guò)觀測(cè)站的參考橢球面的法線與赤道面的夾角。L:大地經(jīng)度，通過(guò)觀測(cè)站的大地子午面與本初子午面得夾角。H:大地高度，觀測(cè)站沿法線到參考橢球面的距離。3.1.3 WGS-84坐標(biāo)系GPS使用的坐標(biāo)系，該坐標(biāo)

5、系屬于協(xié)議地球坐標(biāo)系，其原點(diǎn)位于地球質(zhì)心，Z軸指向BIH1984.0定義的協(xié)議地級(jí)方向，X軸指向BIH1984.0的零子午面和CTP赤道的交點(diǎn)，Y軸與Z軸構(gòu)成右手坐標(biāo)系3。GPS定位結(jié)果屬于協(xié)議地球地心坐標(biāo)系，即84坐標(biāo)系，且通常以空間直角坐標(biāo)（X，Y，Z），或以橢球大地坐標(biāo)（B,L,H）的形式給出。而實(shí)用的常規(guī)地面測(cè)量成果或是屬于國(guó)家的參心大地坐標(biāo)系，或是屬于地方獨(dú)立坐標(biāo)系。因此必須實(shí)現(xiàn)GPS成果的坐標(biāo)系的轉(zhuǎn)換。3.2 坐標(biāo)系轉(zhuǎn)換3.2.1 當(dāng)?shù)厮阶鴺?biāo)系轉(zhuǎn)化為空間直角坐標(biāo)系當(dāng)?shù)厮阶鴺?biāo)系與84坐標(biāo)系同屬于直角坐標(biāo)系，故相互轉(zhuǎn)化時(shí)需要利用地心大地坐標(biāo)系的參數(shù)，將84坐標(biāo)系下（以大地坐標(biāo)（B,

6、L,H）測(cè)得的GPS量作為載體的質(zhì)心，然后經(jīng)過(guò)旋轉(zhuǎn)矩陣將東北天坐標(biāo)系（當(dāng)?shù)厮阶鴺?biāo)系、站心坐標(biāo)系）轉(zhuǎn)化為空間直角坐標(biāo)系。旋轉(zhuǎn)點(diǎn)作為載體的質(zhì)心，以（B,L,H）的值為起點(diǎn)。將坐標(biāo)系分別繞Z、X、Y軸逆時(shí)針旋轉(zhuǎn)、角所使用的旋轉(zhuǎn)矩陣如下：=.(2)= (3)= .(4)由于航位推算是在東北天坐標(biāo)系下進(jìn)行的，所以需要需變換到大地坐標(biāo)系下，顯示經(jīng)緯度信息，先將東北天坐標(biāo)系轉(zhuǎn)換到直角坐標(biāo)系下，然后再有直角坐標(biāo)系轉(zhuǎn)化為大地坐標(biāo)系，將在84坐標(biāo)系（大地坐標(biāo)（B、L、H）形式表示）下測(cè)得的GPS值轉(zhuǎn)化為直角坐標(biāo)系（X、Y、Z）下表示，作為直角坐標(biāo)系下初值，轉(zhuǎn)化公式如下：.(5)現(xiàn)將Y軸反向得軸。繞軸逆時(shí)針旋轉(zhuǎn))

7、，再繞Z軸旋轉(zhuǎn))，即可將東北天坐標(biāo)系（地平站心坐標(biāo)系）轉(zhuǎn)化為站心赤道直角坐標(biāo)系，公式如下：.(6)R=*=所以空間直角坐標(biāo)系和站心赤道直角坐標(biāo)系之間有簡(jiǎn)單的平移關(guān)系，所以將站心赤道坐標(biāo)系轉(zhuǎn)化為空間直角坐標(biāo)系之間的轉(zhuǎn)化關(guān)系為：=.(7)所以與東北天坐標(biāo)系之間的轉(zhuǎn)化關(guān)系為：=(8) 3.2.2 直角坐標(biāo)系轉(zhuǎn)化為大地坐標(biāo)系直角坐標(biāo)系轉(zhuǎn)化為大地坐標(biāo)系的轉(zhuǎn)化公式為：.(8)其中：，N為該點(diǎn)的卯酉圈曲率半徑；，a、b、e分別為該大地坐標(biāo)系對(duì)應(yīng)參考橢球的長(zhǎng)半軸、短半軸和第一偏心率。長(zhǎng)半軸a=63781372m，短半軸b=6356.7523142km，。此方法H和B需要迭代求得，而且應(yīng)用條件受到很大限制，如要

8、求大地高度需要小于1000m時(shí)，才能使B、L迭代四次之后精度分別達(dá)到和0.001m，所以本文一種計(jì)算精度高，公式形式簡(jiǎn)便的方法來(lái)計(jì)算B和L4。.(9)4 電子羅盤(pán)HMR30004.1 概述HMR3000包含三個(gè)磁阻傳感器和一個(gè)雙軸的液體橫滾傳感器，并帶有A/D轉(zhuǎn)換電路和微處理器。微處理器控制傳感器的測(cè)量順序，控制器操作的參數(shù)存儲(chǔ)在EEPROM中，輸出3類(lèi)NMEA0813標(biāo)準(zhǔn)語(yǔ)句(HDG、HDT和XDR)，3類(lèi)專(zhuān)用語(yǔ)句(HPR、RCD和CCD)，HPR是最常用的格式，采用RS232接口功能模塊如圖4-1所示。圖4-1 HMR3000功能框圖4.2 HMR3000特性數(shù)字電子羅盤(pán)HMR3000是H

9、oneywell公司的產(chǎn)品。它可以為導(dǎo)航定位系統(tǒng)提供航向、俯仰、橫滾等數(shù)據(jù)。其中航向的精度為±，分辨率可達(dá)±。橫滾和俯仰的精度為±，分辨率為±，輸出端口采用RS232。HMR3000的串行通信是根據(jù)NMEA0183標(biāo)準(zhǔn)制定的簡(jiǎn)單、異步的ASCII協(xié)議5。ASCII碼的傳輸和接收使用1位起始位、8位數(shù)據(jù)位，無(wú)奇偶校驗(yàn)位和1位停止位。每一個(gè)碼有10位。波特率可選擇1200,2400,4800,9600或38400。HMR3000支持NMEA0183和專(zhuān)用的信息。在測(cè)量模式中有HMR3000按照EEPROM中的編程的速率主動(dòng)發(fā)送NMEA信息。本次試驗(yàn)輸出是專(zhuān)用

10、HPR句子，它包含了航向、俯仰、橫滾等信息。格式如下：HPR Heading, Pitch & Roll（航向，俯仰和橫滾）$PTNTHPR,x.x,a,x.x,a,x.x,a*hh <cr> <lf>這個(gè)句子把HMR3000的三個(gè)重要的測(cè)量結(jié)果和有用的狀態(tài)、俯仰、俯仰狀態(tài)、橫滾、橫滾狀態(tài)。如本次試驗(yàn)的一個(gè)句子為：$PTNTHPR,199.3,N,0.3,N,0.2,N*37此句說(shuō)明：羅盤(pán)所測(cè)航向角為：，正常狀態(tài)，俯仰角為，正常狀態(tài)，橫滾角為，正常狀態(tài)。5 卡爾曼濾波1960年，R.E.Kalman首次提出的卡爾曼濾波是一種線性最小方差估計(jì)?？柭鼮V波理論成功地

11、解決了美國(guó)阿波羅登月中的多導(dǎo)航傳感器組合導(dǎo)航的難題，引起工程界的廣泛重視。之后，卡爾曼濾波理論開(kāi)始在導(dǎo)航系統(tǒng)中得到普遍的應(yīng)用?？柭鼮V波的基本思想：以最小均方誤差為估計(jì)準(zhǔn)則，采用信號(hào)與誤差的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和當(dāng)前的觀測(cè)值來(lái)更新對(duì)狀態(tài)變量的估計(jì)，求出當(dāng)前時(shí)刻的估計(jì)值，算法根據(jù)建立的系統(tǒng)方程和觀測(cè)方程對(duì)需要處理的信號(hào)做出滿足最小均方誤差的的估計(jì)6。由于卡爾曼濾波方程具有易編程的優(yōu)點(diǎn)，所以卡爾曼濾波器在導(dǎo)航方面應(yīng)用十分廣泛，且能夠很好地提高導(dǎo)航的精度?？柭鼮V波是一種最優(yōu)化自回歸數(shù)據(jù)處理方式，在了解狀態(tài)方程、觀測(cè)方程以及系統(tǒng)噪聲的情況下，通過(guò)卡爾曼濾波的五個(gè)公式實(shí)現(xiàn)羅盤(pán)航向角的最優(yōu)

12、化回歸處理，將k-1時(shí)刻的預(yù)測(cè)值以及預(yù)測(cè)偏差和k時(shí)刻的量測(cè)值以及量測(cè)偏差結(jié)合起來(lái)，通過(guò)計(jì)算卡爾曼增益將這5個(gè)式子結(jié)合起來(lái)實(shí)現(xiàn)遞推。下面詳細(xì)介紹一下這五個(gè)式子的含義：首先我們要根據(jù)k-1時(shí)刻的航向角來(lái)預(yù)測(cè)k時(shí)刻的航向角，根據(jù)k-1時(shí)刻的狀態(tài)方程可以得到k時(shí)刻的預(yù)測(cè)航向角如公式(10)：(10)其中是根據(jù)k-1時(shí)刻航向角預(yù)測(cè)k時(shí)刻的航向角，A、B為系統(tǒng)參數(shù)矩陣，為k-1時(shí)刻的航向角，為控制量，此時(shí)為零向量。我們用(covariance)來(lái)表示通過(guò)k-1時(shí)刻方差陣預(yù)測(cè)的k時(shí)刻方差陣（預(yù)測(cè)值的偏差），表示k-1時(shí)刻的航向角的偏差，他們的關(guān)系如公式(11)：(11)Q為過(guò)程噪聲的covariance，假

13、設(shè)k時(shí)刻航向角的量測(cè)值為，現(xiàn)在我們可以得到k時(shí)刻的最優(yōu)化航向角估計(jì)值如公式(12)：(12)H為測(cè)量系統(tǒng)參數(shù)，為k時(shí)刻的卡爾曼增益，求解如公式13：(13)R為觀測(cè)噪聲的covariance，為了能夠以此往下面迭代，故必須求出k時(shí)刻航向角的偏差，即求出。如公式(14)：(1-)*(14)然后再根據(jù)k時(shí)刻的參數(shù)值求解k+1時(shí)刻的航向角，這樣就可以實(shí)時(shí)求解航向角的大小。至于編程時(shí)初始參數(shù)的確定可以任意確定一個(gè)就行，因?yàn)榭柭鼮V波算法會(huì)使這個(gè)參數(shù)逐漸收斂的，但是最好別取零，防止卡爾曼濾波把這個(gè)當(dāng)成最優(yōu)解。實(shí)驗(yàn)中，采用卡爾曼濾波器對(duì)羅盤(pán)輸出的航向值進(jìn)行濾波，濾波器的輸出如圖5.1所示。圖5.1 航向角

14、濾波前后對(duì)比濾波后航向角的相對(duì)誤差如圖5.2所示。圖5.2 濾波數(shù)據(jù)與原始數(shù)據(jù)相對(duì)誤差從圖5.1可以看出，在濾波時(shí)給出的初值和剛開(kāi)始的航向角值差別比較大，卡爾曼濾波能很快收斂到精確的航向角，在羅盤(pán)每次改變方向的時(shí)候，卡爾曼濾波因?yàn)橐鶕?jù)預(yù)測(cè)量和測(cè)量值來(lái)估計(jì)下一時(shí)刻值，所以濾波時(shí)航向角失調(diào)量要大些。6 實(shí)驗(yàn)、數(shù)據(jù)處理及其仿真6.1 實(shí)驗(yàn)內(nèi)容（1）用NT3000GPS接收板卡測(cè)得五分鐘的數(shù)據(jù)，得到一組實(shí)驗(yàn)場(chǎng)地的經(jīng)緯度信息，然后取平均得到一組精度相對(duì)較高的初始經(jīng)緯度信息。通過(guò)串口調(diào)試助手觀測(cè)并保存GPS數(shù)據(jù)為“txt”格式。（2）用HMR3000電子羅盤(pán)繞一周測(cè)4個(gè)方向的航向角信息，每個(gè)方向大概測(cè)5

15、分鐘，通過(guò)串口調(diào)試助手觀測(cè)并保存羅盤(pán)數(shù)據(jù)為“txt”格式。6.2 數(shù)據(jù)處理（1）將GPS測(cè)得經(jīng)緯度信息取平均得到初始B=32.0304，L=118.8574，H=5.3。（2）用matlab讀取羅盤(pán)信息，在東北天坐標(biāo)系下進(jìn)行航位推算，因?yàn)榇牌堑拇嬖谑闺娮恿_盤(pán)存在誤差，所以將測(cè)得的航向角加上當(dāng)?shù)氐拇牌潜愕玫綄?shí)際的航向角（南京的磁偏角為5°），我們這組我取速度為4 m/s,采樣時(shí)間用四組的平均采用時(shí)間，即/4.(15) =(0.1170+0.1157+0.1172+0.1170)/4=0.1167按DR算法可以得到如圖6.1所示運(yùn)動(dòng)軌跡圖：圖6.1 東北天坐標(biāo)系下運(yùn)動(dòng)軌跡從上圖可以看

16、出用濾波后的數(shù)據(jù)進(jìn)行航位推算得到的軌跡與原始數(shù)據(jù)進(jìn)行航位推算得到的軌跡大致相同。說(shuō)明在短時(shí)間內(nèi)羅盤(pán)的導(dǎo)航精度很高。（3）將東北天坐標(biāo)系轉(zhuǎn)化為直角坐標(biāo)系，在直角坐標(biāo)系中推算的軌跡如圖6.2所示，因?yàn)閄-Y面更能清楚的反映運(yùn)動(dòng)情況，故采用平面直角坐標(biāo)系。圖6.2 直角空間坐標(biāo)系X-Y面然后再將直角空間坐標(biāo)系轉(zhuǎn)換為大地坐標(biāo)系，在大地坐標(biāo)經(jīng)緯度面的軌跡如圖6.3所示。圖6.3 大地坐標(biāo)系經(jīng)緯度面在大地坐標(biāo)系（B,L,H）下的軌跡如圖6.4所示。圖6.4 大地坐標(biāo)系X-Y-Z7 誤差分析7.1電子羅盤(pán)誤差電子羅盤(pán)的誤差具體可以分為兩類(lèi)：系統(tǒng)誤差和外界環(huán)境誤差。系統(tǒng)誤差包括羅盤(pán)自身的器件精度誤差以及安裝誤

17、差，主要受羅盤(pán)內(nèi)部的三軸磁阻傳感器影響。外界環(huán)境誤差是指羅盤(pán)工作的環(huán)境所帶來(lái)的誤差，如載體傾斜帶來(lái)的誤差、磁羅差誤差等。本實(shí)驗(yàn)中羅盤(pán)放在計(jì)算機(jī)、示波器和工控機(jī)旁邊易受到干擾，所以羅差對(duì)羅盤(pán)的精度有很大的影響。雖然濾波后得到的軌跡比濾波前精確些，但是羅盤(pán)每個(gè)方向的具體方位角并不精確，若想提高每個(gè)方向的方位角精度，最好能對(duì)羅盤(pán)進(jìn)行補(bǔ)償，可以在因?yàn)檩d體傾斜帶來(lái)的誤差和磁羅差方面分別進(jìn)行補(bǔ)償，因?yàn)楸緦?shí)驗(yàn)中沒(méi)有理想的精確的方位角理論值，所以沒(méi)有對(duì)羅盤(pán)進(jìn)行補(bǔ)償，只對(duì)羅盤(pán)轉(zhuǎn)動(dòng)的軌跡進(jìn)行了仿真。7.2 航位推算本身誤差航位推算一個(gè)致命的弱點(diǎn)就是誤差隨著時(shí)間而積累。所以推算的航向角是發(fā)散的，航向角誤差越來(lái)越大。

18、因此航位推算不能長(zhǎng)時(shí)間的單獨(dú)工作，必須借助于其他的定位方式進(jìn)行組合導(dǎo)航，比如GPS/DR組合導(dǎo)航，航位推算借助于GPS定期傳來(lái)的載體位置進(jìn)行聯(lián)合導(dǎo)航，精度更加精確。8總結(jié)在得到一組GPS導(dǎo)航數(shù)據(jù)的基礎(chǔ)上利用航位推算進(jìn)行導(dǎo)航，通過(guò)卡爾曼濾波對(duì)航向角數(shù)據(jù)進(jìn)行處理，減小誤差。利用matlab仿真出分別在東北天坐標(biāo)系、直角坐標(biāo)系、大地坐標(biāo)系下的運(yùn)動(dòng)軌跡，載體運(yùn)動(dòng)軌跡與實(shí)驗(yàn)時(shí)的羅盤(pán)向軌跡相同。最后分析了航位推算導(dǎo)航定位的誤差原因，并且希望以后在羅盤(pán)補(bǔ)償方面能做更進(jìn)一步的研究。參考文獻(xiàn)1.孫希延, 紀(jì)元法與施滸立, 卡爾曼濾波實(shí)現(xiàn)車(chē)載GPS/DR組合導(dǎo)航. 現(xiàn)代電子技術(shù), 2006(11): 第149-1

19、52頁(yè).2.吳富梅, 楊元喜與田育民, GPS/DR組合導(dǎo)航自適應(yīng)Kalman濾波算法. 測(cè)繪科學(xué)技術(shù)學(xué)報(bào), 2008(3): 第206-208+212頁(yè).3.王珂, GPS測(cè)向算法與應(yīng)用研究, 2009, 重慶大學(xué).4.張華海，鄭南山，王軍，李景芝. 由空間直角坐標(biāo)計(jì)算大地坐標(biāo)的簡(jiǎn)便公式.全球定位系統(tǒng). 2002，4:9-125.彭樹(shù)生, 數(shù)字電子羅盤(pán)HMR3000的特性及應(yīng)用. 電子技術(shù), 2004(8): 第39-43頁(yè).6.付夢(mèng)印，鄧忠紅，張繼偉.Kalman 濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用M.北京:科學(xué) 出版社，2003.附錄clear all;load head.matZ=head

20、ing;m,n=size(Z);T=0.1167;A=1 T;0 1;%系統(tǒng)參數(shù)矩陣（狀態(tài)轉(zhuǎn)移矩陣）H=1 0; %測(cè)量系統(tǒng)參數(shù)X=100;0; %狀態(tài)變量初值C=diag(1,1);%誤差協(xié)方差初值Rk=diag(1);%量測(cè)噪聲方差Ta=T;1;%狀態(tài)過(guò)程噪聲陣sigam=1;Qk=Ta*Ta'*sigam2;%過(guò)程噪聲方差初值f=Z(:,1);for k=1:m Xk=A*X;%從k-1時(shí)刻預(yù)測(cè)k時(shí)刻 Ck=A*C*A'+Qk;%k-1時(shí)刻預(yù)測(cè)k時(shí)刻方差 K=Ck*H'*inv(H*Ck*H'+Rk);%kalman增益 X=Xk+K*(Z(k,:)-H*

21、Xk);%狀態(tài)更新 C=Ck-K*H*Ck;%方差更新 Xe(:,k)=X;%濾波輸出 endfigure(1)plot(Xe(1,:),'r');hold on;plot(heading,'b');grid on;hold on;B=32.0304;L=118.8574;H=5.3;e2=0.00673949674227;format short e1=(63781402-63567552)/63781402;N=6378140/(sqrt(1-e1*(sind(B)2); X=(N+H)*cosd(B)*cosd(L);Y=(N+H)*cosd(B)*sin

22、d(L);Z=(N*(1-e1)+H)*sind(B); c=Xe(1,:);n3 n4=size(Xe);for i=1:(n4+1) if i=1 x2(i)=0; y2(i)=0; else d1=c(i-1)+5; x2(i)=x2(i-1)+4*0.1167*cosd(d1); y2(i)=y2(i-1)+4*0.1167*sind(d1); end endfor i=1:(n4+1) if i=1 x3(i)=0; y3(i)=0; else d2=f(i-1)-5; x3(i)=x3(i-1)+4*0.1167*cosd(d2); y3(i)=y3(i-1)+4*0.1167*sind(d2); end end for i=1:(n4)d3(:,i)=abs(f(i)-c(i)/f(i);endfigure(2)plot(d3(1,:),'r');grid on;hold on;figure(3) plot(x2,y2,x3,y3,'r')hold on;xlabel('正東/(m)');ylabel('正北/(m)')