https://github.com/leonardodalinky/kalmanfilter

卡尔曼滤波器的实现。内附原理及说明。基于numpy矩阵运算。
https://github.com/leonardodalinky/kalmanfilter

Last synced: 3 months ago
JSON representation

卡尔曼滤波器的实现。内附原理及说明。基于numpy矩阵运算。

• Host: GitHub
• URL: https://github.com/leonardodalinky/kalmanfilter
• Owner: leonardodalinky
• License: mit
• Created: 2020-05-05T14:12:26.000Z (over 4 years ago)
• Default Branch: master
• Last Pushed: 2020-05-05T14:24:33.000Z (over 4 years ago)
• Last Synced: 2024-10-03T20:41:44.645Z (3 months ago)
• Language: Python
• Size: 5.86 KB
• Stars: 6
• Watchers: 1
• Forks: 4
• Open Issues: 0
• Metadata Files:
  • Readme: README.md
  • License: LICENSE

Awesome Lists containing this project

README

        
# KalmanFilter

卡尔曼滤波器的实现。内附原理及说明。基于numpy矩阵运算。  

## 说明

对于一个线性系统的状态差分方程为  

	$$x_k = A * x_{k-1} + B*u_{k-1} + w_k$$  

其中$x$是系统的状态向量，大小为$n*1$列。$A$为转换矩阵，大小为$n*n$。  

$u$为系统输入，大小为$k*1$。$B$是将输入转换为状态的矩阵，大小为$n*k$。  

随机变量$w$为系统噪声。注意这些矩阵的大小，它们与你实际编程密切相关。  

测量值当然是由系统状态变量映射出来的，方程形式如下：  

	$$z_k = H*x_k + v_k$$  

注意，此处的$x_k$表示的是仍是上面的系统预测值，这个测量值的映射只不过是理想情况下。  

注意$Z$是测量值，大小为$m*1$($m$取决于测量值），$H$也是状态变量到测量的转换矩阵。大小为$m*n$。随机变量$v$是测量噪声。  

首先要计算预测值、预测值和真实值之间误差协方差矩阵。  

	(1) $$\hat{x}_k^{-} = A * \hat{x}_{k-1} + B * u_{k-1}$$  

	(2) $$P_k^{-} = A * P_{k-1} * A^T + Q$$  

其中 $\hat{x_k^{-}} $和$ P_k^{-} $表示预测的结果和预测的协方差矩阵。  

$Q$表示系统噪声  

有了这两个就能计算卡尔曼增益K，再然后得到估计值，  

	(3) $$K_k = P_k^{-} * H^T * (H * P_k^{-} * H^T + R)^{-1}$$  

	(4) $$\hat{x}_k = \hat{x}_k^{-} + K_k * (z_k - H * \hat{x}_k^{-})$$  

其中 $K_k$ 为卡尔曼增益，$R$ 表示传感器噪声  

最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。  

	(5)$$ P_k = (I - K_k * H) * P_k^{-}$$  

$I$ 表示单位矩阵  

建议此markdown文档以本地下载形式打开，并开启latex公式编译功能。

## 源码说明

在源码中，附有完善的Python Doc。变量名与上面说明相对应。