Student_3_KF_MeasureUpdate.m

功能：扩展卡尔曼滤波EKF的测量更新
% 输入：
% GPSDat——当前时刻GPS定位定速结果，维数：1*7，时刻（s）、位置（m，n0系） 和速度（m/s，n系）；
% MIMURes—— SINS惯导积分解算结果结构体，其元素有：
% MIMURes.MIMUTime 当前时刻 MIMU测量数据的时刻值
% MIMURes.dt 当前时刻积分时间间隔（s）
% MIMURes.Cb2n 当前时刻 MIMU所在载体b系到当地NED n系的方向余弦矩阵
% MIMURes.Qua 当前时刻 n系到 b系的旋转四元数
% MIMURes.Att 当前时刻 n系到 b系的旋转欧拉角（321转序），滚动角、俯仰角、偏航角，量纲：rad ；
% MIMURes.Vel 当前时刻 b系相对于 n系的平动速度，当地北东地坐标系 n系，单位：m/s
% MIMURes.BLH 当前时刻 b系所在位置的大地坐标，纬度(rad)、经度(rad)和高度(m)
% NavEarthPar——与导航相关的地球物理参数结构体，有9个元素：
% NavEarthPar.Grav_n n系中载体所在位置的地球重力加速度矢量（m/s^2）
% NavEarthPar.w_ien e系相对于 i系的旋转角速度矢量在 n系中的值（rad/s）
% NavEarthPar.w_enn n系相对于 e系的旋转角速度矢量在 n系中的值（rad/s）
% NavEarthPar.w_inn n系相对于 i系的旋转角速度矢量在 n系中的值（rad/s）
% NavEarthPar.Rn 载体所在位置地球参考椭球子午圈半径（m）
% NavEarthPar.Re 载体所在位置地球参考椭球卯酉圈半径（m）
% NavEarthPar.CB 载体所在位置大地纬度 B的余弦值
% NavEarthPar.SB 载体所在位置大地纬度 B的正弦值
% NavEarthPar.TB 载体所在位置大地纬度 B的正切值

% ErrState——当前时刻误差状态预报值，15*1数组，数据格式：
% 第1-3行——纬度误差（rad）、经度误差（rad）和高程误差(m)；
% 第4-6行——速度误差(m/s),当地北东地坐标系 n系 ；
% 第7-9行——失准角(rad)，n系至失准后的 n系；
% 第10-12行——陀螺偏差(rad/s)；
% 第13-15行——加表偏差(m/s^2)。
% ErrStateCov—————————当前时刻误差状态预报值协方差矩阵，15x15矩阵，量纲与误差状态ErrState的对应。

% 输出：
% ErrState————————— 当前时刻误差状态测量更新值，15*1数组，数据格式：
% 第1-3行——纬度误差（rad）、经度误差（rad）和高程误差(m)；
% 第4-6行——速度误差(m/s),当地北东地坐标系 n系 ；
% 第7-9行——失准角(rad)，n系至失准后的 n系；
% 第10-12行——陀螺偏差(rad/s)；
% 第13-15行——加表偏差(m/s^2)。
% ErrStateCov————————— 当前时刻误差状态测量更新值协方差矩阵，15x15矩阵，量纲与误差状态ErrState的对应。