卡尔曼滤波分析及程序

这部分内容不注重卡尔曼滤波的推导，只关注卡尔曼滤波的作用【据说推导很难，总之这篇文章是对卡尔曼的整体流程进行解释，没有纠结公式的逐步推导，看了这部分会让你知道卡尔曼滤波是干什么的，具体的步骤是什么✔✔】

卡尔曼滤波是干什么的

​ 卡尔曼滤波是用来对事物的状态进行估计的，为什么要对事物的状态进行估计呢？那肯定是我们无法准确的知道事物的当前状态，这时候我们就要进行估计了嘛【但是我们不可能是随便的估计，往往是根据一些经验等来进行估计，所有估计的结果是相对准确的】；另一方面，为了估计一个事物的状态，我们还可以借助一些仪器来进行测量，但是用仪器进行测量就会存在一定的测量误差。这时候就会产生一个问题，我们应该相信我们的估计结果还是相信测量结果呢？我想大概率人们都会这么想：如果我们使用的测量仪器是从某个小厂家手里花五毛钱买的劣质仪器，那么我们会更相信估计结果；如果买的仪器价值千金，那我们则有理由相信测量结果。其实呢，我们往往都会给这两种结果分配不同的比例，表示我们更信任哪种结果。那么卡尔曼滤波就是结合事先的估计和仪器的测量这两种手段来估计 事物状态的方法。

​ 我们再举个例子来说明上文所要表达的含义。如下图所示，我们用炉子给水壶烧水，现在水沸腾了，我们结合初中知识，估计水的温度是95.4°C【受大气压影响，没有一百度】，即Xk=95.4°C。这时候我们用一个温度计测量水的温度，发现其值为yk=94.1°C。发现xk和yk不一致，该相信谁呢？我们假设温度计的可信度为0.6，估计的为0.4，则最终的的水温=0.6*94.1+0.4*95.4=94.62。

卡尔曼滤波例子分析

卡尔曼公式

卡尔曼主要就是上图中的五个公式，首先是对物体状态进行预测，然后进行测量更新，如此循环。

卡尔曼滤波直观理解

​

卡尔曼Python例子【使用jupyter-notebook】

该例子主要是随机生产一些位置和速度信息，然后通过卡尔曼滤波估计出行人速度和位置。

#载入相关的库
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
from scipy.stats import norm

#初始化行人状态x, 行人的不确定性（协方差矩阵）P，测量的时间间隔dt，处理矩阵F以及测量矩阵H：
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
print(x, x.shape)
P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])
print(P, P.shape)

dt = 0.1 # Time Step between Filter Steps
F = np.matrix([[1.0, 0.0, dt, 0.0],
              [0.0, 1.0, 0.0, dt],
              [0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(F, F.shape)

H = np.matrix([[0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(H, H.shape)

ra = 10.0**2

R = np.matrix([[ra, 0.0],
              [0.0, ra]])
print(R, R.shape)

#计算测量过程的噪声的协方差矩阵R和过程噪声的协方差矩阵Q
ra = 0.09
R = np.matrix([[ra, 0.0],
              [0.0, ra]])
print(R, R.shape)

sv = 0.5
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2
from sympy import Symbol, Matrix
from sympy.interactive import printing
printing.init_printing()
dts = Symbol('dt')
Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[dts],[dts]])
Qs*Qs.T

I = np.eye(4)
print(I, I.shape)

#随机产生一些测量数据
m = 200 # Measurements
vx= 20 # in X
vy= 10 # in Y

mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))
measurements = np.vstack((mx,my))

print(measurements.shape)
print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx))
print('You assumed %.2f in R.' % R[0,0])

fig = plt.figure(figsize=(16,5))
plt.step(range(m),mx, label='$\dot x$')
plt.step(range(m),my, label='$\dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best',prop={'size':18})

#一些过程值，用于结果的显示
xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []

def savestates(x, Z, P, R, K):
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0,0]))
    Py.append(float(P[1,1]))
    Pdx.append(float(P[2,2]))
    Pdy.append(float(P[3,3]))
    Rdx.append(float(R[0,0]))
    Rdy.append(float(R[1,1]))
    Kx.append(float(K[0,0]))
    Ky.append(float(K[1,0]))
    Kdx.append(float(K[2,0]))
    Kdy.append(float(K[3,0]))    

#卡尔曼滤波
for n in range(len(measurements[0])):
 
    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = F*x
    
    # Project the error covariance ahead
    P = F*P*F.T + Q
    
    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)
    
    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x)                            # Innovation or Residual
    x = x + (K*y)
    
    # Update the error covariance
    P = (I - (K*H))*P
    
    # Save states (for Plotting)
    savestates(x, Z, P, R, K)

#显示一下关于速度的估计结果
def plot_x():
    fig = plt.figure(figsize=(16,9))
    plt.step(range(len(measurements[0])),dxt, label='$estimateVx$')
    plt.step(range(len(measurements[0])),dyt, label='$estimateVy$')
    
    plt.step(range(len(measurements[0])),measurements[0], label='$measurementVx$')
    plt.step(range(len(measurements[0])),measurements[1], label='$measurementVy$')

    plt.axhline(vx, color='#999999', label='$trueVx$')
    plt.axhline(vy, color='#999999', label='$trueVy$')

    plt.xlabel('Filter Step')
    plt.title('Estimate (Elements from State Vector $x$)')
    plt.legend(loc='best',prop={'size':11})
    plt.ylim([0, 30])
    plt.ylabel('Velocity')
plot_x()

#显示位置的估计结果
def plot_xy():
    fig = plt.figure(figsize=(16,16))
    plt.scatter(xt,yt, s=20, label='State', c='k')
    plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
    plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')

    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Position')
    plt.legend(loc='best')
    plt.axis('equal')
plot_xy()