扩展卡尔曼滤波EKF的测量更新

功能：扩展卡尔曼滤波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系