由于需要对经纬度信息进行卡尔曼滤波,而且系统状态含有速度信息,所以要使用基于米定位的坐标系。故而需要先把经纬度信息转为UTM信息。
需要保证输入的观察值Ck的UTM区域要与当前拍的先验估计值保持在同一个区域(否则坐标会出现跳变),需要在每次完成滤波后对后验值判定当前所在区域。
若输入的区域与前一拍的后验值不在一个区域时,直接舍弃当前拍的后验值,把当前观测设定为前一拍的后验,重新初始化滤波。这种做法虽然牺牲临界点的经度但是比较方便。
UTM坐标系概念详解:(建议直接看这个英文的维基百科,中文没有找到讲的很好的)
https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system
MATLAB 的转换程序
(双向)https://www.mathworks.com/matlabcentral/fileexchange/45699-ll2utm-and-utm2ll
(deg2utm)