卡尔曼滤波应该是本专业最最基础的内容之一,但是一直不是很熟悉,最近在上蔡远利教授的课,有相关的作业,所以决定认真掌握一下。
基本思路:
新的最佳估计基于原最佳估计和已知外部影响校正后得到的预测。
新的不确定性基于原不确定性和外部环境的不确定性得到的预测。
数学理论:
示例:
- 代码
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()
-
结论