这篇文章主要讲解了“python实现kalman滤波的方法”,文中的讲解内容简单清晰,易于学习与理解,下面请大家跟着小编的思路慢慢深入,一起来研究和学习“python实现kalman滤波的方法”吧!
卡尔曼滤波的本质是对最小二乘法的迭代运算,可以给出时间序列的状态估计。假设其要估计的过程如下:
x[k+1] = A[k]*x[k] + B*u[k] + w[k] // 状态方程 z[k] = H[k]*x[k] + v[k] // 测量值 p(w) ~ N(0, Q) p(v) ~ N(0, R)
其中w和v代表满足正态分布的噪音项,该正态分布均值为0,协方差矩阵分别为Q和R。u代表对x的控制项,z代表测量值,k+1和k代表不同时刻的值。A,B,H分别为相应的关联矩阵。卡尔曼滤波将该过程的预测值分为两部分,一是通过模型对先验值的推断,称为时间更新;二是通过测量值进行修正,称为测量更新。其核心方程为:
// 时间更新 xb[k+1] = A[k]*x[k] + B*u[k] Pb[k+1] = A[k]*P[k]*transverse(A[k]) + Q[k] // 测量更新 K[k] = Pb[k]*transverse(H[k])*inverse(H[k]*Pb[k]*transverse(H[k])+R[k]) x[k] = xb[k] + K*(z[k] - H[k]*xb[k]) P[k] = (I - K[k]*H[k])*Pb[k]
其中xb为状态先验估计值,Pb为先验误差协方差矩阵,P为后验误差协方差矩阵。在每次时间更新中,利用前一个后验估计值给出下一时刻的先验估计值xb,并给出下一个时刻的先验误差协方差估计。在每次测量更新中,先计算出卡尔曼增益K,然后利用测量值z和先验估计值xb计算出当前的后验估计值x,最后再给出当前的后验误差协方差估计。
这两个更新过程融合了先验估计(从过去的数据和模型推断的系统状态)和可能存在噪音的测量值,从而给出了系统最有可能的状态(分布)。该方法的优点在于,在测量和控制都不够精确的情况下,给出结合二者数据的最佳估计。下面给出简单的python代码及运行结果供参考。
import numpy class Kalman_Filter: def __init__(self, A, H, Q, R, z, B = None, impulse = None): self._A = A self._H = H self._Q = Q self._R = R self._z = z self.m = len(z) self.n = len(z[0]) self._identity = numpy.ones([self.n, self.n]) if (B is None): self._B = numpy.zeros([self.n, self.n]) else: self._B = B if (impulse is None): self._impulse = numpy.zeros([self.m, self.n]) else: self._impulse = impulse def __del__(self): return def _kalman(self, xb, Pb, z, impulse): # 测量更新 tmp = numpy.matmul(Pb, self._H.T) K = numpy.matmul(tmp, numpy.linalg.inv(numpy.matmul(self._H, tmp) + self._R)) x = xb + numpy.matmul(K, (z - numpy.matmul(self._H, xb))) P = numpy.matmul((self._identity - numpy.matmul(K, self._H)), Pb) # 时间更新 xb = numpy.matmul(self._A, x) + numpy.matmul(self._B, impulse) Pb = numpy.matmul(numpy.matmul(self._A, P), self._A.T) + self._Q return x, xb, Pb def _kalman1d(self, xb, Pb, z, impulse): # 测量更新 tmp = Pb*self._H K = tmp/(self._H*tmp + self._R) x = xb + K*(z - self._H*xb) P = (1 - K*self._H)*Pb # 时间更新 xb = self._A*x + self._B*impulse Pb = self._A*P*self._A + self._Q return x, xb, Pb def get_filtered_data(self, xb, Pb): xx = [] for i in range(0, self.m): if (self.n == 1): (x, xb, Pb) = self._kalman1d(xb, Pb, self._z[i], self._impulse[i]) else: (x, xb, Pb) = self._kalman(xb, Pb, self._z[i], self._impulse[i]) xx.append(x) return xx # =========== test =============== import matplotlib.pyplot t = numpy.linspace(0,10,100) # 横坐标,时间 # ================= 2d ================== A = numpy.array([[1,0.1], [0,1]]) H = numpy.array([[1,0],[0,1]]) Q = 0.5*numpy.array([[1,0],[0,1]]) R = 0.5*numpy.array([[1,0],[0,1]]) noise = numpy.random.randn(2, 100) real = numpy.vstack((10*numpy.sin(t), 10*numpy.cos(t))) # 真实值 z = real + noise # 测量值 kf = Kalman_Filter(A, H, Q, R, z.T) xb = numpy.array([0,10]) Pb = numpy.array([[1,0],[0,1]]) x = kf.get_filtered_data(xb, Pb) fig = matplotlib.pyplot.figure(figsize=(10.24,7.68)) matplotlib.pyplot.plot(t, z.T, 'r') matplotlib.pyplot.plot(t, real.T, 'g') matplotlib.pyplot.plot(t, x, 'b') matplotlib.pyplot.show() # =================== 1d ================= A = 1 H = 1 Q = 0.5 R = 0.5 B = -1 # 根据反馈进行修正 noise = numpy.random.randn(1, 100) real = 10*numpy.exp(-t*t) z = real + noise kf1 = Kalman_Filter(A, H, Q, R, z.T) # 不加反馈 kf2 = Kalman_Filter(A, H, Q, R, z.T, B, noise.T) # 反馈修正 xb = 10 Pb = 1 x1 = kf1.get_filtered_data(xb, Pb) x2 = kf2.get_filtered_data(xb, Pb) fig = matplotlib.pyplot.figure(figsize=(10.24,7.68)) matplotlib.pyplot.subplot(3,1,1) # 下面画第一个图,不带反馈修正 matplotlib.pyplot.plot(t, z.T, 'r') matplotlib.pyplot.plot(t, real.T, 'g') matplotlib.pyplot.plot(t, x1, 'b') matplotlib.pyplot.subplot(3,1,2) # 下面画第二个图,带反馈修正 matplotlib.pyplot.plot(t, z.T, 'r') matplotlib.pyplot.plot(t, real.T, 'g') matplotlib.pyplot.plot(t, x2, 'b') matplotlib.pyplot.subplot(3,1,3) # 下面画第三个图,比较带反馈和不带反馈的结果 matplotlib.pyplot.plot(t, x1, 'r') matplotlib.pyplot.plot(t, real.T, 'g') matplotlib.pyplot.plot(t, x2, 'b') matplotlib.pyplot.show()
计算结果如下。可以看到,在大部分情况下,蓝线(Kalman滤波结果)要比红线(测量值)更加接近绿线(真实值)。在第二个图中,对比了加入外部反馈以根据测量结果进行修正和不加的情况。可以看到,增加反馈后在某些较大的值处给出比较好的结果,在0点附近震荡更加均匀。但反馈无法在所有位置都改善结果。
感谢各位的阅读,以上就是“python实现kalman滤波的方法”的内容了,经过本文的学习后,相信大家对python实现kalman滤波的方法这一问题有了更深刻的体会,具体使用情况还需要大家实践验证。这里是亿速云,小编将为大家推送更多相关知识点的文章,欢迎关注!
免责声明:本站发布的内容(图片、视频和文字)以原创、转载和分享为主,文章观点不代表本网站立场,如果涉及侵权请联系站长邮箱:is@yisu.com进行举报,并提供相关证据,一经查实,将立刻删除涉嫌侵权内容。