求助UDUT滤波程序

我设计了一个导航程序，用标准卡尔曼滤波结果达不到精度要求，问师兄采用UDUT滤波可以满足要求，可自己看着算法有点复杂，问哪位大哥帮帮小弟。
邮箱：jack722204@tom.com