控制之美（卷2）第六节——卡尔曼滤波器的实际仿真

Zhao_kar

控制之美（卷2）第六节——卡尔曼滤波器的实际仿真 [复制链接]

控制之美（卷2）第六节——卡尔曼滤波器的实际仿真

声明：

• 本节是结合前面的理论的一个实例仿真，后续最后一节再补充一个矩阵求导的实例
• 本节以无人机的高度观测为例展示，即为本书的代码仿真

仿真：

• 首先状态空间方程根据之前的章节已知：Xk+1=Axk+Buk，这里不写了
• 接着构建一个矩阵，也就是A，这个是系统矩阵A = [0 1 0; 0 0 1 ;0 0 0];
• 然后再构建输入的，其中三个变量，本例以高度、速度、重力加速度
• 再把过程噪声w补上，也就是最开始的方程，前面的文章提到了
• 此时要考虑第二个测量zk，所以接下来是Hm，也就是测量矩阵，定义H_m = [1 0 0; 0 1 0; 0 0 1];
• 采样时间设为0.1s，两个协方差分别设为
• R_c = [1 0 0; 0 1 0; 0 0 0];
• Q_c = [0.01 0 0; 0 0.01 0; 0 0 0];
• 同时要换成离散系统，sys_d = c2d(ss(A,B),Ts);使用c2d函数进行转换
• 然后设定初始值，本例为一个0，1，-10，也就是说高度0，1m/s的速度上升，加速度直接当成10
• 然后设定输入为g，也就是加速度抵消，实则为一个匀速上升的运动
• 接下来就是设定后验的三个参数，以及运行次数，然后跑个循环
• 本例运行步数设定是100次，定义过程噪声协方差矩阵 Q_c 和测量噪声协方差矩阵 R_c
• 然后是初始化，有两个
• 初始化系统状态 x0 和后验估计 x_hat0
• 初始化后验估计误差协方差矩阵 P0
• 定义系统状态和测量的噪声矩阵 w 和 v
• 然后数据是随机生成的，这个不用管
• 定义完之后就可以跑循环仿真了，主要的过程如下：
• 计算系统的实际状态 x 和测量值 z
• 使用线性卡尔曼滤波器（函数 F8_LinearKalmanFilter）更新状态估计和估计误差协方差，这个实际上是作者在前面讲算法的时候的一个函数文件
• 将系统状态、测量值、先验估计和后验估计保存到相应的矩阵中
• 这部分的代码就不放出来了，总之可以得到一个系统，详见仿真图，通过卡尔曼滤波器的使用，可以有效地对无人机高度进行预测，并且能够减少传感器测量误差和系统噪声对预测结果的影响

• 这里可以顺便看看工作区的变量数值，其实稍微对比一下是可以发现规律是对的，预测结果没有什么问题