% 函数功能： SRUKF滤波器 % 适用范围： 测量线性、噪声加性时的SRUKF 滤波 % INPUT % x1： 系统状态 % T： 捷联矩阵计算值 % Tbn： 捷联矩阵真实值 % fp： 比力在计算坐标系内的投影 % Pw： 过程噪声 % Pv： 量测噪声 % phi： 计算的纬度 % % OUTPUT % Xk: 滤波后的状态 %