⛄一、拓展卡尔曼滤波时序四旋翼无人机状态跟踪
卡尔曼滤波算法为获得最优估计和最小误差方差,将从目标模型中得到的测量值一步步递推,实时获取新时刻的状态估计值。
假设目标状态方程和观测方程分别为:
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/e329814c8c67c0d1005c0e78b5be73c8.png)
其中,k为离散时间,X(k)为系统在k时刻的状态,Y(k)为观测信号,W(k)为过程噪声,V(k)为观测噪声,A是状态转移矩阵,B是噪声驱动矩阵,H是观测矩阵。假设W(k)和V(k)是均值为零,方差各为Q和R的不相关白噪声。
目标在k时刻的估计值,即卡尔曼滤波算法的数学表达式[12]如下:
状态一步预测为:
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/b3a0a3bb2c22137cc34df5da84e74c8c.png)
协方差一步预测为:
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/f148d23f47b7274a2babd5512ac23f25.png)
卡尔曼滤波增益为:
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/4bce2c60fa24c4b0bb6b929fc71db71b.png)
状态更新为: