北航卡尔曼滤波课程-捷联惯导静基座初始对准实验

卡尔曼滤波实验报告捷联惯导静基座初始对准实验一、实验目的①掌握捷联惯导的构成和基本工作原理；②掌握捷联惯导静基座对准的基本工作原理；③了解捷联惯导静基座对准时的每个系统状态的可观测性；④了解双位置对准时系统状态的可观测性的变化。二、实验原理选取状态变量为：[]TENENUxyxyzXVV，其中导航坐标系选为东北天坐标系，EV为东向速度误差，NV为北向速度误差，E为东向姿态误差角，N为北向姿态误差角，U为天向姿态误差角，x为东向加速度偏置，y为北向加速度偏置，x为东向陀螺漂移，y为北向陀螺漂移，z为天向陀螺漂移。则系统的状态模型为：XAXW(1)其中，1112212211121321222331323302sin000002sin000000000sincos0000sin000000cos000000000000000000000000000000000000000000000000000000LgCCLgCCLLCCCLCCCLCCCA[00000]ENENUTVV，EDVWW为零均值高斯白噪声，分别为加速度计误差和陀螺漂移的噪声成分，为地球自转角速度，ijC为姿态矩阵nbC中的元素，L为当地纬度。量测量选取两个水平速度误差：[]TENZVV，则量测方程为：10000000000100000000EENNVXV(2)即ZHX其中，H为量测矩阵，TEN为量测方程的随机噪声状态矢量，为零均值高斯白噪声。要利用基本卡尔曼滤波方程进行状态估计，需要将状态方程和量测方程进行离散化。系统转移矩阵为：2323/1111102!3!!nnkkkkkknTTTITAAAAn(3)其中，T为采样间隔。量测矩阵为：10000000000100000000kH(4)所以经过离散化，系统的数学模型为：,1111kkkkkkkkkkXXWZHXV(5)其中，系统噪声驱动阵1kI。有了状态方程和量测方程，可以采用以下滤波方程进行卡尔曼滤波：/11,1/1,11,11111/1/1/1/1/1()()()()kkkkkTTkkkkkkkkkkTTkkkkkkkkkkkkkkkkkTTkkkkkkkkkkXXPPQKPHHPHRXXKZHXPIKHPIKHKRK(6)滤波方程中所用的量测值kZ——速度误差，可由捷联惯导采用角增量法解算。三、静基座初始对准流程陀螺加计测量数据解析粗对准开始捷联惯导解算卡尔曼滤波对准完毕结束NY修正图1静基座初始对准流程图四、实验结果及分析4.1仿真数据单位置初始对准仿真条件：陀螺常值漂移:0.02°/h；陀螺随机漂移:0.01°//h；加速度计常值偏置:100ug；加速度计随机误差:50ug；初始失准角ΨN，ΨE，Ψ惯导所处位置的地理纬度：L=45°仿真结果：0200400600800100012001400160018002000-0.02-0.0100.01东向速度误差时间/s速度误差/m/s020040060080010001200140016001800200000.020.040.060.08东向速度误差均方差时间/s速度误差/m/s0200400600800100012001400160018002000-50510x10-3北向速度误差时间/s速度误差/m/s020040060080010001200140016001800200000.020.040.060.08北向速度误差均方差时间/s速度误差/m/s0200400600800100012001400160018002000-0.500.51东向水平失准角时间/s误差角/deg020040060080010001200140016001800200000.0050.010.0150.02东向水平失准角均方差时间/s误差角/deg020040060080010001200140016001800200000.51北向水平失准角时间/s误差角/deg020040060080010001200140016001800200000.0050.010.0150.02北向水平失准角均方差时间/s误差角/deg020040060080010001200140016001800200000.511.5平台方位失准角时间/s误差角/deg020040060080010001200140016001800200000.0050.010.0150.02平台方位失准角均方差时间/s误差角/deg0200400600800100012001400160018002000-0.8-0.6-0.4-0.20东向加速度计偏置时间/s偏置/μg02004006008001000120014001600180020009.769.779.789.79x10-4东向加速度计偏置均方差时间/s偏置/μg0200400600800100012001400160018002000-2-1.5-1-0.50北向加速度计偏置时间/s偏置/μg02004006008001000120014001600180020009.77859.7799.77959.789.7805x10-4北向加速度计偏置均方差时间/s偏置/μg图2仿真数据单位置初始对准结果4.2真实数据双位置初始对准仿真条件：采样频率：100Hz陀螺常值漂移:0.01度/每小时陀螺随机漂移：0.01度/每小时加速度计常值偏置：50ug加计随机误差：50ug初始位置：纬度：39.97912经度：116.34681高度：100.38米对准结果：0200400600800100012001400160018002000-50510x10-4东向陀螺漂移时间/s漂移/deg/h02004006008001000120014001600180020000.0050.010.0150.02东向陀螺漂移均方差时间/s漂移/deg/h0200400600800100012001400160018002000-3-2-101x10-3北向陀螺漂移时间/s漂移/deg/h02004006008001000120014001600180020000.0190.01950.020.0205北向陀螺漂移均方差时间/s漂移/deg/h0200400600800100012001400160018002000-6-4-202x10-4天向陀螺漂移时间/s漂移/deg/h02004006008001000120014001600180020000.01940.01960.01980.02天向陀螺漂移均方差时间/s漂移/deg/h050010001500-0.03-0.02-0.0100.01东向速度误差时间/s速度误差/m/s05001000150000.020.040.060.08东向速度误差均方差时间/s速度误差/m/s050010001500-0.0200.020.04北向速度误差时间/s速度误差/m/s05001000150000.020.040.060.08北向速度误差均方差时间/s速度误差/m/s05001000150000.511.5东向水平失准角时间/s误差角/deg05001000150000.51x10-3东向水平失准角均方差时间/s误差角/deg050010001500-0.500.51北向水平失准角时间/s误差角/deg05001000150000.51x10-3北向水平失准角均方差时间/s误差角/deg0500100015000.70.80.91平台方位失准角时间/s误差角/deg0500100015007.588.59x10-4平台方位失准角均方差时间/s误差角/deg050010001500-200-1000100200东向加速度计偏置时间/s偏置/μg05001000150000.51x10-3东向加速度计偏置均方差时间/s偏置/μg050010001500-300-200-1000100北向加速度计偏置时间/s偏置/μg05001000150000.51x10-3北向加速度计偏置均方差时间/s偏置/μg050010001500-0.06-0.04-0.020东向陀螺漂移时间/s漂移/deg/h0500100015000.010.0150.02东向陀螺漂移均方差时间/s漂移/deg/h05001000150000.050.10.150.2北向陀螺漂移时间/s漂移/deg/h0500100015000.010.0150.02北向陀螺漂移均方差时间/s漂移/deg/h图3真实数据双位置初始对准结果05001000150000.0050.010.015天向陀螺漂移时间/s漂移/deg/h0500100015000.020.020.020.02天向陀螺漂移均方差时间/s漂移/deg/h