python 滤波_卡尔曼滤波及基于Python的简单实现

卡尔曼滤波应该是本专业最最基础的内容之一，但是一直不是很熟悉，最近在上蔡远利教授的课，有相关的作业，所以决定认真掌握一下。

基本思路：

新的最佳估计基于原最佳估计和已知外部影响校正后得到的预测。

新的不确定性基于原不确定性和外部环境的不确定性得到的预测。

数学理论：

卡尔曼滤波算法

示例：

昆虫问题

代码

from numpy import *

import pylab

import math

# 参数初始化

n_iter = 100

sz = (n_iter,) # size of array

# 分配数组空间

x = zeros(sz)

xes = zeros(sz)

f = zeros(sz)

fes = zeros(sz)

kk1 = zeros(n_iter-1)

kk2 = zeros(n_iter-1)

A = mat([[0.5,2],[0,1]])

B = mat([[0],[1]])

C = mat([1,0])

P = mat([[500,0],[0,200]])

Q = mat([[0,0],[0,10]])

I = mat([[1,0],[0,1]])

U = random.normal(0,math.sqrt(10),n_iter)

V = random.normal(0,math.sqrt(10),n_iter)

# 真实值

x[0]=650

f[0]=250

# 估计值

xes[0]=600

fes[0]=200

R = 10

# intial guesses

xhat = mat([[650],[250]])

for k in range(1,n_iter):

# 一步预测

xhat = A*xhat + B*U[k-1] #X(k|k-1) = AX(k-1|k-1) + BU(k)

y = C*xhat + V[k-1]

P = A*P*A.T + Q

x[k] = xhat[0][0]

f[k] = xhat[1][0]

# 量测修正

inv = linalg.inv(C*P*C.T+R)

K = P*C.T*inv #Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R]

kk1[k-1] = K[0][0]

kk2[k-1] = K[1][0]

xhat = xhat+K*(y-C*xhat) #X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)]

xes[k] = xhat[0][0]

fes[k] = xhat[1][0]

P = (I-K*C)*P #P(k|k) = (1 - Kg(k)H)P(k|k-1)

print(P)

pylab.figure()

pylab.plot(x,'k*',label='simulation system')

pylab.plot(xes,'b^',label='estimation')

pylab.title('The number of insects')

pylab.xticks(arange(0,n_iter,1))

pylab.legend()

pylab.xlabel('Iteration')

pylab.ylabel('quantity')

pylab.figure()

pylab.plot(f,'k*',label='simulation system')

pylab.plot(fes,'b^',label='estimation')

pylab.title('The number of food')

pylab.xticks(arange(0,n_iter,1))

pylab.legend()

pylab.xlabel('Iteration')

pylab.ylabel('quantity')

pylab.figure()

pylab.plot(kk1,'k-',label='insects')

pylab.plot(kk2,'b-',label='food')

pylab.title('Kalman gain curve')

pylab.xticks(arange(0,n_iter-1,1))

pylab.legend()

pylab.xlabel('Iteration')

pylab.ylabel('quantity')

pylab.show()

结论

昆虫数量随时间演化示意图

食物供给量随时间演化示意图

卡尔曼增益曲线