基本思路：

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

数学理论：

卡尔曼滤波算法

示例：

昆虫问题

代码

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()