Measure_Update_for_Student_3_KF.m

简介：
该文件是针对学生作业设计的一个卡尔曼滤波器（KF）测量更新函数的Matlab实现代码，适用于学习和理解状态估计及跟踪技术。 功能：扩展卡尔曼滤波EKF的测量更新 输入： - GPSDat——当前时刻GPS定位定速结果，维数为1*7，包括时间（s）、位置（m，n0系） 和速度（m/s，n系）。 - MIMURes——SINS惯导积分解算结果结构体。其元素有：MIMUTime 当前时刻MIMU测量数据的时刻值；dt当前时刻积分时间间隔（s）；Cb2n 当前时刻MIMU所在载体b系到当地NED n系的方向余弦矩阵；Qua 当前时刻n系到 b系的旋转四元数；Att 当前时刻n系到 b系的旋转欧拉角（321转序），包括滚动角、俯仰角和偏航角，量纲为rad ；Vel 当前时刻b系相对于 n系的平动速度，在当地北东地坐标系n系中，单位为m/s；BLH 当前时刻 b系所在位置的大地坐标（纬度(rad)、经度(rad) 和高度(m)）。 - NavEarthPar——与导航相关的地球物理参数结构体。包含9个元素：Grav_n n系中载体所在位置的地球重力加速度矢量（m/s^2）；w_ien e系相对于 i系的旋转角速度矢量在n 系中的值（rad/s）； w_enn n系相对于 e系的旋转角速度矢量在n 系中的值（rad/s）；w_inn n系相对于 i系的旋转角速度矢量在n 系中的值（rad/s）；Rn 载体所在位置地球参考椭球子午圈半径（m）；Re 载体所在位置地球参考椭球卯酉圈半径（m）；CB 载体所在位置大地纬度 B的余弦值；SB 载体所在位置大地纬度 B的正弦值；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的对应。