SCKF简单测试代码。仅用于学习，禁止用于其他用途，转发注明出处作者。