INSGPS组合导航系统仿真分析

INS/GPS组合导航系统仿真分析INS/GPS组合导航系统程序流程度开始读入和设定初始值t是否在导航解算周期内t是否为组合导航时刻读入GPS接收机数据F矩阵离散化Kalman滤波输出估计结果给出前一时刻的陀螺与加速度计的值导航解算结束组合导航量测值YesNONoYesX－＋初值的选取在程序开始时，必须要有初值才能进行卡尔曼滤波。卡尔曼滤波要求初值条件为：这样卡尔曼滤波才具有无偏性和估计均方差最小的特性。卡尔曼滤波要求系统噪声和量测噪声是互不相关的零均值的白噪声序列，有：其中：00^00000XXXXXmmPCPC{}{}TkjkkjTkjkkjEWWQEVVR,kkkkWkVkQR时刻的系统噪声时刻的量测噪声系统噪声和量测噪声的方差矩阵，卡尔曼滤波要求它们分别是已知的非负定阵和正定阵组合导航系统误差方程当组合系统采用线型卡尔曼滤波器时，常常利用间接法进行估计，所谓“系统”实际上是导航系统的各种误差的“组合体”，滤波器估值的主要部分是导航参数误差估值，然后用去校正惯导系统INS的导航参数。惯性导航系统的误差方程包括：平台误差角方程；速度误差方程；位置误差方程等。.X.X平台误差角方程惯性导航系统的平台系要模拟的导航坐标系为当地地理坐标系(t),由于平台有误差，平台系(P)和导航系(t)不能完全重合，平台系(P)与地理系(t)总存在小姿态误差角将平台误差角写成(t)系中的列向量有：地理系(t)和平台系(P)之间的坐标变换阵，取一阶近似，可写成：在捷联惯导系统中，由于误差的影响理想的当地地理系与计算所得的地理系有小角度误差，坐标变换阵与上式完全一样，即：平台式INS中，平台相对于惯性空间的转动角速度为：亦可写成:TtENU111UNptUENEC'tpttCC..1pptttttiptititC.1tpttipit平台误差角方程又其中：上式有关项可写成：于是平台误差角方程的矢量表达式为：综合以上平台误差角方程的矩阵表达式可写成：pppipicpicp施矩角作用下的跟踪转动陀螺漂移引起的平台等效误差转动pttticitieettttitieet.ttttpttieetieet＝－...0sintancossintan0cos0sinEEieiennENENienmUNEienmNmENNienUEnvvLLLRhRhvvLLRhRhvvLRhRhvRhvLLRhvRh2tancossecENUEienvLLLLRh速度误差方程及位置误差方程速度误差方程矩阵表达式为：位置误差方程：根据直接写出误差方程为：..00tan2sin2cos2sin0UNEEENNUENNEEieiemnnENNEienmffvffvvvvLLLRhRhRhvvvvLRhRh..sec,NEnnvvLLRhRh..secsectanNnEEnnvRhLvLvLLLRhRh组合导航系统的状态方程忽略天向速度误差和高度误差，INS/GPS组合导航系统的状态变量为：系统的噪声向量为：陀螺，加速度计误差考虑为白噪声，即：所以系统状态误差方程如下：,,,,,,,,,,,,TENUENbxbybzxyzXVVL,,,,,0,0,0,0,0,0,0,0TENUaEaNW,,,ENUaEaN陀螺沿东，北，天方向的随机漂移加速度计沿东，北方向的随机漂移.00TEWtWtQt.＝，＝.sintancostan1,11,21,3NEEEieNieUmnntttbbxbbybbzEVVVLLLLRhRhRhCCC组合导航系统的状态方程..2sinsintan2,12,22,3tancoscossec3,13,23,3NEENieieEUnnmtttbbxbbybbzNNEUieENEnmntttEiebbxbbybbzUnVVVLLLLRhRhRhCCCVVLLVRhRhRhVLLLCCCRh.2.2tan2sintan2cossec1,11,21,32sintan2cossecNEEUNNUEieNmntttENieNbxbybzaEnENUEEUieEntEieEbnVVVffLVLLVRhRhVVLVLLCCCRhVVffLLVRhVLLVLCRh2,12,22,3ttxbybzaNCC组合导航系统的状态方程........1sectansec000000NmEEnnbxbybzxyzLVRhVLLLVLRhRh组合导航系统的状态方程上述各式中，下标E、N、U代表东、北、天的坐标轴方向；综合以上列举的方程，误差模型可写成：,,,,,,,,,,,,ENUENUENbxbybzxyzieENUVVVVVLfff东、北、天方向平台误差角东、北、天方向的速度沿东、北方向的速度误差纬度误差，经度误差陀螺沿东、北方向的常值漂移加速度计沿东、北方向的常值漂移地球自传角速度沿东、北、天、方向的加速度计感受到的比力.XtFtXtWt系统量测方程INS的位置量测信息可表达为地理系下的真值与相应误差之和：GPS接收机给出的位置量测信息，可表示为地理系下的真值与相应误差之差：式中：不考虑高度通道，位置量测矢量为：式中：tIItItLLLhhhcosNtmGEGtnGtUNLRLNRLhhN,,,,GPStttENULhNNN真实位置接收机沿东、北、天方向的位置误差cosNIGmpppIGEnNLLLRZtHtXtVtNRL25262130110cospTNEpnHdiagNNVRRLINS的的速度量测信息，可表示为地理系下的真值与相应的速度误差之和：GPS的速度量测信息，同样可表示为地理系下的真值与相应的测速误差之差：不考虑天向速度，速度量测矢量为：式中：,,IEEEINNNIUUUENUVVVVVVVVVVVVt机体沿地理系()东、北、天坐标轴的真实速度,,GPSGEEEGNNNGUUUENUVVMVVMVVMMMM接收机的测速误差项再东、北、天坐标轴上的分量IEGEEEVVVNNINGNVVVMZtHtXtVtVMVV23282130110VVENHdiagVMMpPPVVvZtHtVtZtXtHtXtVtHtVtZt2243462241301100110cosTNEENmndiagHtdiagNNVtMMRRL其中：状态转移阵系统的状态方程和观测方程为：其中：.EXAXGWZHXV1,2sintan1,3cos11,51,81,11,91,21,101,32,1sintan2,3nmxieNnmxieNMnbnbnbnmxieNnmyMVALLRhVALRhARhACACACVALLRhVARh212,42,6sin2,82,12,92,22,102,33,1cos3,213,4tan3,6cossec3,83,13,93,23,103,3NienbnbnbnmxieNnmyMNnmxieNnbnbnbARhALACACACVALRhVARhALRhVALLRhACACAC24,24,34,4tan4,5tan2sin4,62cossec2sin4,111,14,121,24,131,35,15,35,42tansinmznmynnmymzNNnmxieNnnmxmynniemyiemzNnbnbnbnmznmxnmxieNAfAfVVALRhRhVALLRhVVAVLLVLRhACACACAfAfVALRhn5,5nmzMLVARh25,62cossec5,112,15,122,25,132,316,5sec7,47,6sectannnmxiemxNnbnbnbMNnmxNVALLVRhACACACARhLARhVALLRhA除上述元素外，其余均为零