飞行器空间坐标修正(含程序)

1、题目： 飞行器空间坐标修正 【摘 要】什么是惯性导航或惯性制导呢? 惯性导航系统（INS，以下简称惯导）是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。但测量所得的数据也存在一定的误差，误差主要有陀螺的漂移、标度因数误差、加速度计的零偏和标度因数误差、计算机的舍入误差、电流变换装置的误差等，惯性导航系统的积累误差随着时间的推移而逐渐增大，这一问题严重影响到航空航天技术

2、的发展。因此进一步研究飞行器空间坐标修正方法有重要的理论意义和应用价值。而在本文，我们主要从电子仪器的精度和噪声干扰、陀螺的漂移误差累计造成的误差入手，结合牛顿运动定理及逆向求值原理建立合理的自回归模型AR模型及卡尔曼滤波模型对数据进行修正。而卡尔曼滤波模型正是本文的重心，因为利用卡尔曼滤波来以减小陀螺仪随机漂移对系统精度的影响，效果较佳。自回归AR（1）模型主要建立在Excel各种统计函数运用的基础上，通过统计各组已知数据残差、方差、标准偏差等，发现数据之间的潜在关系，然后根据加速度计测量值随时间的变化相对较小的特点、从正向和反向建立关系模型：然后取两次平均值以减小误差。卡尔曼过滤中信号和噪

3、声是用状态方程和测量方程来表示的。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。关键词：随机误差、AR自回归、逆向、Excel、卡尔曼滤波、误差修正、Matlab、一、问题重述1.1问题背景 惯性导航系统是一种不依赖于任何外部信息的自主式导航系统，在航空航天领域起着越来越重要的作用。由于其系统结构误差、惯性测量部件误差、标度系数误差等因素的影响，惯性导航系统的积累误差随着时间的推移而逐渐增大，这一问题严重影响到航空航天技术的发展。因此进一步研究飞行器空间坐标修正方法有重要的理论意义和应用价

4、值。1.2已知条件某一观测站（0，0,0）测得飞行器空间位置(假设观测站为坐标原点)X(x、y、z)，飞行器的飞行速度V(x轴、y轴、z轴),飞行器与观测站之间的偏向角，俯仰角以及观测数据的时间间隔t。所给的各项数据均含有一定的误差，其中1.3问题的提出1、 飞行器坐标的数据为观测值，由于电子仪器的精度和噪声干扰等，含有一定的误差波动，建立数学模型对飞行器坐标观测值的随机波动误差进行修正。2、 由于观测数据的仪器误差，飞行器坐标在长时间的飞行中，坐标数据的观测值由于误差的累积发生漂移，建立数学模型，对飞行器的坐标的这种误差进行修正。(提示：在短时间内，可以视为飞行器坐标含有一定的常量误差，或者

5、飞行器的这种误差是线性变化的)。3、 结合具体的飞行器给出误差修正方案。 二、问题分析对于问题一，属于随机误差的修正问题，因为题目有说明飞行器的坐标（观测值）可能含有较大误差，所以我们运用自回归模型 AR（p）模型（p代表AR模型的阶数）,来对误差进行修正，直接使用最简单的一阶模型：并根据牛顿运动定理大胆定出正向模型和逆向模型求得两组数据，再去两次求得的数据的平均值作为最后修正值，最后我们可通过检验,发现其有正有负，符合误差波动原理，说明模型可靠； 对于问题二、三，因为选择用卡尔曼过滤，是因为卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。它不需要知道全部过去的数据，采用递推的方法计算，

6、它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统。三、模型假设1、速度的误差相比于坐标数据，误差较小；2、在同一个时间序列中将速度看作相等；3、过程及测量的噪声，被假设成高斯白噪声；4、假设和不随系统状态变化而变化。5、飞行器不包含系统噪声；四、符号说明符号符号说明符号符号说明飞行器在时的X轴坐标测量的噪声，假设成白噪声通过逆推算出的时的X轴坐标k时刻的状态估计通过正推算出的时的X轴坐标的covariance问题一修正后的X轴坐标的covariance行器在时的X轴方向的速度的covariance问题一修正后的Y轴坐标问题一修正后的Z轴坐标k时刻的系统状态k-1时刻到k时

7、刻的状态转移矩阵k时刻的测量值测量系统的参数过程的噪声，假设成高斯白噪声五、模型建立及求解5.1问题一模型建立及求解根据自回归模型 AR（p） 模型（自回归模型, p代表AR模型的阶数）,结构式：3我们可以简化模型为一阶自回归模型AR(1)：2该模型代表“一阶马尔科夫”过程的平稳随机过程，这种随机过程的特点是：随机过程在t 时刻的观测值仅与相邻的前一时刻（t-1）时刻的观测值 存在相关性。 加速度计是极易受外部干扰的传感器，但是测量值随时间的变化相对较小，只是随着时间的增大，由于积分误差的积累，所以所测得的坐标值的精确度也就越来越小。但在短时间内，时间的变化量对积分的值影响较小，故可以得到较为

8、精确的飞行器的空间坐标，可通过速度积分得到。 由常理可知，当飞行器里观测点越近，测量结果会越精确，再结合牛顿运动定理，所以为了获得更为精确的初始位置，我们选择分别令最后一次的测量结果为初始位置，通过 逆向推算可得一组x的坐标；再通过 正向推算可得另一组x的坐标，然后取平均值，获得最终的修正值。 故可以得到修正模型： 同理可得、，、，运用统一修正的方法： 3 对y轴、z轴进行修正，得到下表，下表为坐标修正后的值：tx'y'h'tx'y'h'01360.2941309.567510.23023.151155.4981089.015510.30820.

9、151350.511299.009510.28373.31145.7491078.51510.11690.31340.741288.473510.21123.451136.0011067.993510.19160.451330.9921277.971510.17633.61126.2591057.501510.1890.61321.251267.515510.25693.751116.5211046.982510.17770.751311.5041257.022510.10913.91106.7531036.491510.26030.91301.7791246.488510.39974.051

10、096.9761026.029510.16811.051291.9781235.974510.12624.21087.2381015.52510.18111.21282.2351225.505510.28684.351077.5031004.985510.10731.351272.5151215.022510.154.51067.764994.5092510.06841.51262.7611204.504510.21544.651058.01984.0044510.04291.651252.9861194.01510.15614.81048.261973.4881510.23751.81243

11、.2381183.485510.33394.951038.479963.0125510.12071.951233.5181172.986510.37095.11028.725952.4961510.27472.11223.7341162.519510.32175.251019.02941.9697510.26972.251213.9761152.001510.42855.41009.266931.5158510.29272.41204.2781141.505510.2625.55999.507921.017510.19882.551194.5031131.005510.39225.7989.7

12、399910.4792510.24972.71184.7561120.494510.40285.85979.9724899.9979510.30722.851175.0111109.991510.30086970.2128889.4539510.32253.01165.2171099.481510.36415.2问题二、三模型建立与求解1、卡尔曼滤波简介卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制

13、，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出，其信号模型是从状态方程和量测方程得到的。卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。设飞行器动态系统的状态方程和测量方程分别为： 上两式子中，是k时刻的系统状态，是k-1时刻到

14、k时刻的状态转移矩阵，是k时刻的测量值，是测量系统的参数，和分别表示过程和测量的噪声，他们被假设成高斯白噪声。进一步预测： (1)状态估计： (2) 滤波增益矩阵： (3) 一步预测误差方差阵： (4) 估计误差方差阵： (5)上述就是卡尔曼滤波器的5条基本公式，只有给定初值和，根据k时刻的观测值，就可以递推计算得k时刻的状态估计（K=1，2，N）。我们将飞行器的飞行状态看成一组向量(状态量)，所以有：通过观察站所观测到的观测值也看作是一组向量观测数据为：其中矩阵A是状态转移矩阵， 为过程噪音，为采样次数，H为状态转移矩阵，为观测噪音；根据牛顿定律可得：为采样间隔，所以：我们把和的covari

15、ance记为和（这里我们假设他们不随系统状态变化而变化），把的covariance 记为，估测值的状态方程为：观测方程为：估测前的covariance为：现在状态的最优化估算值：卡尔曼增益(Kalman Gain)：迭代后的： 通过卡尔曼滤波得到结果如下图，修正得到估计值，部分数据下表，全部数据见附录表格。 图一 表一通过图，我们可以看出修正效果明显。因为卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。所以问题三，我们可以结合具体的飞行器

16、，直接用卡尔曼对其进行修正。7六、模型的检验问题一模型的检验，主要是通过修正后的飞行器的坐标，通过积分求得近似速度，发现比原有数据所得的速度更接近测量速度。并通过求出修正值与原有数据进行比较，发现原有数据在修正数据上下波动。所以模型相对可靠。问题二卡尔曼滤波后，我们可以通过实用Matlab检验：输入：X=X'plot(T,X(:,3),'*-',T,num(:,3),'-o')得到： 图二 图三为h的滤波效果图，绿色为原数据，蓝色为滤波后的数据，可见滤波效果明显。 同理可对x、y进行检验，皆可发现效果明显，故模型可靠 图三七、模型的评价7.1模型优点 问

17、题一中的AR一阶自回归模型较为简单，并结合了牛顿运动定理，有一定的实际意义；同时根据距离越近惯性导航系统测量的值误差越小，运用正向、逆向获取两组数据，再取平均值，这样有利于减小误差；且求得的结果符合波动原理，符合题意。 卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态，较为实用。7.2模型不足与改进 自回归AR模型是以误差相对较小的速度反推飞行器坐标，得到的结果依旧存在误差，只能说是减小了随机误差，修正效果还不够好。所以问题二使用用较为精确的卡尔曼滤波模型，改进模型。参考文献【1】 叶其孝、姜启源，数学建模，北京：机械工业出版社

18、，2009.8【2】 盛骤、谢式千、潘承毅，概率论与数理统计(第四版)，北京：高等教育出版社,2008.6【3】 姜启源、谢金星、叶俊，数学模型，北京：高等教育出版社，2011.1【4】 韩明、王家宝、李林，数学实验（MATLAB版），上海：同济大学出版社，2012.1【5】 万俊、潘鸿飞、杨柏军，陀螺仪随机漂移的测取和数学模型的确立【6】 佚名，卡尔曼滤波简介+算法实现代码， 2013.5.1【7】 佚名，惯性导航系统， 2013.4.29 97附录：1.X修正结果逆推x值正推x值修正x值X-修正x值1360.251360.3381360.294-0.0441350.6263881350.3

19、931350.51-0.0097915471340.8174031340.6621340.740.0100771641331.110981330.8741330.9920.0077040021321.3180071321.1831321.25-0.0003018591311.6184291311.3891311.504-0.0037246411301.8768191301.6821301.779-0.0294060241292.0748361291.8821291.9780.0217965711282.3475021282.1231282.2350.0146587531272.60574127

20、2.4251272.515-0.0154521311262.8704121262.6521262.761-0.0114550121253.0779951252.8941252.9860.0138725491243.3467431243.131243.2380.0118345511233.6134521233.4221233.518-0.0177285921223.8144711223.6531223.7340.0161355951214.0653581213.8871213.9760.0240468381204.3713611204.1861204.278-0.0284448631194.57

21、1241194.4351194.503-0.0029407491184.8829731184.6291184.756-0.0058059751175.0940351174.9291175.011-0.011397461165.316261165.1171165.2170.0333564281155.589321155.4061155.4980.0023572461145.8145351145.6841145.7490.0008624491136.091261135.9111136.001-0.0009698121126.3318991126.1851126.259-0.008681734111

22、6.6338051116.4091116.521-0.021272411106.8379311106.6681106.753-0.0030159621097.0854621096.8661096.9760.0241713871087.3139511087.1621087.2380.0119899781077.5917131077.4151077.503-0.0031253331067.8413311067.6861067.764-0.0136900161058.1125821057.9081058.01-0.0104348681048.3623581048.1591048.261-0.0105

23、135531038.5696011038.3871038.4790.0214906631028.8124451028.6381028.7250.0249564261019.1101881018.931019.02-0.0202935341009.3445541009.1881009.266-0.016054692999.6241642999.3898999.507-0.006988031989.8243193989.6554989.73990.010117584980.0690459979.8758979.97240.02755917970.25970.1757970.21280.037159

24、6391.Y值修正结果：正推y值逆推y值修正y值Y-修正值1309.51309.6341309.567-0.067051298.889061299.1281299.009-0.008781288.36591288.581288.4730.0272291277.87151278.071277.9710.0291281267.420361267.6091267.515-0.01471256.929761257.1141257.022-0.021691246.390971246.5851246.4880.0119851235.886371236.0621235.9740.0256691225.414

25、941225.5961225.505-0.005471214.937711215.1061215.022-0.022021204.404011204.6031204.504-0.003631193.893681194.1261194.01-0.010081183.396751183.5731183.4850.015331172.873511173.0981172.9860.0144771162.427411162.6111162.519-0.019281151.902461152.0991152.001-0.000791141.388851141.6211141.505-0.005081130

26、.900881131.1091131.005-0.005171120.378681120.6091120.4940.0063211109.890531110.0921109.9910.0085211099.391331099.571099.4810.0193431088.907571089.1221089.015-0.014961078.430011078.591078.51-0.009981067.877651068.1091067.9930.0065441057.410061057.5921057.501-0.001211046.890741047.0731046.9820.0183631

27、036.407631036.5751036.4910.0087571025.927461026.1311026.029-0.029421015.425141015.6151015.52-0.020241004.868631005.1011004.9850.015141994.384661994.6337994.5092-0.00919983.89891984.11984.0044-0.00444973.366271973.61973.48810.01187962.890025963.1349963.0125-0.01248952.39001952.6021952.49610.00395941.

28、865069942.0743941.96970.030324931.39791931.6336931.5158-0.01577920.925718921.1084921.017-0.01704910.366377910.5921910.47920.020752899.891643900.1042899.99790.002072889.407881889.5889.45390.0460591.Z值修正值：正推z值逆推z值修正z值Z-修正值510.352510.1083510.23020.1218371510.4739510.0936510.2837-0.071586510.316510.1065

29、510.21120.0037301510.3363510.0163510.17630.0221029510.2903510.2234510.2569-0.148854510.1997510.0185510.10910.2265361510.4478510.3516510.3997-0.316072510.1488510.1036510.12620.3159332510.5327510.0409510.2868-0.083305510.3033509.9966510.150.0092874510.2777510.1532510.2154-0.147288510.1397510.1725510.1

30、5610.0914419510.3418510.326510.3339-0.053634510.3881510.3536510.37090.0368396510.4894510.1539510.32170.1110666510.5118510.3453510.4285-0.188784510.3256510.1983510.2620.1884731510.5556510.2288510.3922-0.099198510.3877510.418510.4028-0.089839510.3972510.2045510.30080.1903386510.5643510.1639510.3641-0.

31、074093510.3756510.2408510.3082-0.066063510.3204509.9135510.11690.1980037510.3891509.9942510.1916-0.166867510.136510.2419510.189-0.086227510.2113510.144510.17770.1399346510.3933510.1274510.2603-0.01078510.3551509.9811510.16810.0770827510.363509.9993510.1811-0.076904510.2274509.9872510.1073-0.00428251

32、0.2067509.9301510.06840.0026527510.1549509.9309510.0429-0.016855510.122510.3531510.2375-0.205831510.1325510.109510.12070.3087469510.5059510.0436510.2747-0.075421510.2896510.2497510.2697-0.119015510.2577510.3276510.29270.0314043510.3985509.999510.19880.228277510.5265509.973510.2497-0.161173510.178151

33、0.4364510.3072-0.24816510.1452510.4997510.32250.1772949卡尔曼滤波程序代码：clc;clearnum=xlsread('data.xls',1,'C3:J43');Z=num't=0.15;A =1 0 0 t 0 0 0 0; 0 1 0 0 t 0 0 0; 0 0 1 0 0 t 0 0; 0 0 0 1 0 0 0 0; 0 0 0 0 1 0 0 0; 0 0 0 0 0 1 0 0; 0 0 0 0 0 0 1 0; 0 0 0 0 0 0 0 1; H=eye(8);X=Z;I=H;P

34、= H;R=H;Q=H; N=length(Z); %要测量的点数 ； for k=2:N X(:,k)=A*X(:,k-1);K=P*H'*inv(H*P*H'+R);X(:,k)=X(:,k-1)+K*(Z(:,k)-H*X(:,k-1);P=A*P*A'+Q;P=(I-K*H)*P;endX %显示信号x(k)的数据；m=1:N; n=m*0.15;plot(n,Z,'-r*',n,X,'-bo'); %便于比较zk和x(k)在同一窗口输出；legend('观测数据z(t)','信号估计值x(t)',

35、2);grid; X Y h alpha theta vx vy vz1360.251309.5510.3520.794395-0.2490965.7120370.73957-0.812191355.3751304.25510.28210.794532-0.2499765.6478770.81678-0.752111348.0321296.342510.2520.521672-0.5453665.7438470.83671-0.78051339.451287.1510.21950.344053-0.7384765.5960270.68382-0.696561330.2831277.228510

36、.1660.234009-0.8589665.6656970.57605-0.6541320.8241267.041510.24760.168223-0.9318765.5629570.65146-0.700991311.221256.698510.16910.130022-0.9751465.6729170.70449-0.56771301.5431246.277510.30990.108336-1.0006865.7639870.63579-0.585751291.8291235.816510.25140.096272-1.0158965.6298470.52553-0.625711282

37、.0981225.336510.20530.089704-1.0252265.638270.58274-0.707561272.3571214.846510.13740.086229-1.0312565.6689570.64579-0.592191262.6121204.351510.19540.084475-1.0354765.7376970.66707-0.610451252.8641193.853510.23420.083672-1.038765.6306970.75517-0.664651243.1151183.355510.32190.08339-1.041465.64170.619

38、56-0.604591233.3661172.855510.37990.083394-1.0438365.7005570.6349-0.565991223.6161162.356510.30430.083554-1.0461365.5629770.68794-0.569111213.8661151.856510.37560.083801-1.0483765.5022170.67438-0.63511204.1161141.356510.33960.084097-1.050665.653770.74158-0.633161194.3661130.856510.32290.084422-1.0528465.5640770.73569-0.59721184.6161120.356510.41370.084765-1.055165.7279470.73009-0.542571174.8661109.856510.34950.085121-1.057396