个人写的卡尔曼滤波方程推导的验证算法,效率不高也不节省内存,仅仅是演示+学习+验证使用,建议配合教程食用。