1. 主页
  2. 文档
  3. 现代控制理论
  4. 状态观测器
  5. 卡尔曼滤波器

卡尔曼滤波器


卡尔曼滤波器有什么作用?

卡尔曼滤波器被用来进行最优状态估计,它能够使系统状态的方差达到最小,利用了随机噪声为正态分布这一假设,估计出随机信号中所需的信号。

如果你要处理的信号是随机信号,或者需要进行多个传感器数据的融合,那么可以考虑选择卡尔曼滤波器。


卡尔曼滤波器与全维状态观测器

闭环全维状态观测器离散化后为:

\(\hat{x}_{k+1}=A\hat{x}_{k}+Bu_{k}+H(y_{k}-\hat{y}_{k})\)

\(\hat{y}_{k}=C\hat{x}_{k}\)

将第二式代入第一式得:

\(\hat{x}_{k+1}=A\hat{x}_{k}+Bu_{k}+H(y_{k}-C\hat{x}_{k})\)

其中决定观测器性能的是矩阵H,而卡尔曼滤波通过最小化方差这一指标迭代计算H,H最后会收敛,使结果达到最优

可以对比下,卡尔曼滤波器为:

\(\hat{x}_{k}=A\hat{x}_{k-1}+Bu_{k}+K_{k}(y_{k}-C(A\hat{x}_{k-1}+Bu_{k}))\)

由于系统的定义不同,下标可能略有不同。


卡尔曼滤波器的五个方程

首先定义系统模型:

\(x_{k}=Ax_{k-1}+Bu_{k}+w_{k}\)

\(y_{k}=Cx_{k}+v_{k}\)

其中,w是系统随机噪声,假设满足均值为0,方差为Q的正态分布v是量测随机噪声,假设满足均值为0,方差为R的正态分布

通过最小化状态值误差值的协方差可以得到以下五个方程:

系统方程用以预测:

\(\hat{x}^{-}_{k}=A\hat{x}_{k-1}+Bu_{k}\)

\(P^{-}_{k}=AP_{k-1}A^{T}+Q\)

量测方程用以修正:

\(K_{k}=\frac{P_{k}^{-}C^{T}}{CP_{k}^{-}C^{T}+R}\)

\(\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}(y_{k}-C\hat{x}_{k}^{-})\)

\(P_{k}=(I-K_{k}C)P_{k}^{-}\)

卡尔曼滤波器的估计值是模型和量测量的加权平均值,与噪声方差有关,噪声方差R越大所占权重越小,越不被观测器信任


程序参考

该程序包采用C语言编写,用vs2013编译,由两个卡尔曼滤波程序组成:分别用以一维状态变量一维输出变量以及任意维状态变量一至二维输出变量的滤波。

  • kalman,一维状态变量,一维输出变量;
  • kalman_filter,任意维状态变量,一至二维输出变量;

程序采用结构体卡尔曼滤波器进行封装,需要在主函数中定义系统参数,噪声协方差卡尔曼滤波器状态初值以及状态误差协方差矩阵初值

程序有效性已经过验证,如有bug请联系我们以便及时修改。

下载链接请关注微信公众平台iccetop,回复“卡尔曼滤波”进行获取



这篇文章对您有用吗?

我们要如何帮助您?

发表评论

邮箱地址不会被公开。 必填项已用*标注