上半年毕设的时候接触了卡尔曼滤波器，用matlab实现了该过程，尝试在一个课后作业中用三维度矩阵来存储变量的方式，结构似乎更好理解，记录一下分析的过程。
假如有一块电阻，你不知道它的阻值是多少，你想通过多次测量电压和电流值，从而用定义法求出来它的阻值大小，测量结果如下表所示：Current (A)Voltage (V)
0.21.23
0.31.38
0.42.06
0.52.47
0.63.17
但实际上，每次测量都有一些‘噪声’，因此修正的公式应该是这样 = +
如果在一张图片上显示这些坐标的话，
import numpy as np
from numpy.linalg import inv
import matplotlib.pyplot as plt
I = np.array([[0.2, 0.3, 0.4, 0.5, 0.6]]).T
V = np.array([[1.23, 1.38, 2.06, 2.47, 3.17]]).T
plt.scatter(I, V)
plt.xlabel('Current (A)')
plt.ylabel('Voltage (V)')
plt.grid(True)
plt.show()如果采用最小二乘法的话，可以得到这样的图像。
## Batch Solution
H = np.ones((5, 2))
H[:, 0] = I.ravel()
x_ls = inv(H.T.dot(H)).dot(H.T.dot(V))
print('The slope and offset parameters of the best-fit line (i.e., the resistance and offset) are [R, b]:')
print(x_ls[0, 0])
print(x_ls[1, 0])
# Plot line.
I_line = np.arange(0, 0.8, 0.1).reshape(8, 1)
V_line = x_ls[0]*I_line + x_ls[1]
plt.scatter(I, V)
plt.plot(I_line, V_line)
plt.xlabel('Current (A)')
plt.ylabel('Voltage (V)')
plt.grid(True)
plt.show()那么应用卡尔曼滤波器该如何操作呢？
思考一下矩阵的维度： | 变量 | 维度 | |--|--| | U | (5,1) | | I | (5,1) | | x_k | (2,1) | |P_k | (2,2) | |R_k | (1,1) | |x_hist| (6,2) | |P_hist | (6,2,2) | |H_k | (5,2) | |K_k | (2,5) |
因此，我们来检查一下更新过程中这三组的维数是否正确。
K_k = P_hist[k,:,:].dot(H_k.T).dot(np.linalg.pinv(H_k.dot(P_hist[k,:,:]).dot(H_k.T) + R_k))
P_hist[k,:,:]选择了P_hist的第k行，而它是(6,2,2)的矩阵，因此其维数是(2,2),即取出了一行(1,2,2). 因此K_k的计算如下：分子部分的维度是 (2,2)和(2,5)相乘，为(2,5)
分母部分 (5,2),(2,2),(2,5)相乘，再加上R_k(广播机制)，为(5,5)
那么最后是(2,5)与(5,5)相乘，为(2,5)的矩阵
x_k = x_k + K_k.dot( V/I - H_k.dot(x_k))
x_k是(2,1)的矩阵，V/I是(5,1)的矩阵，H_k.dot(x_k)是 (5,2）与(2,1)相乘，结果为(5,1)的矩阵，那么再和K_k相乘，结果是(2,5)与(5,1)相乘，是(2,1),因此更新后的x_k同样为(2,1)的矩阵，计算正确。
P_k = (1 - K_k.dot(H_k)).dot(P_hist[k,:,:])
K_k.dot(H_k)是(2,5)的矩阵和(5,2)的相乘为(2,2)，而P_hist[k,:,:]为(2,2),因此更新后P_k依然是(2,2)，更新正确。
还有一个细节就是说，只用X_hist和P_hist存储数据，其他变量不存储只做更新作用，即不断的将新的值填充到X和P中，想象P是一个立方体，不断的从上到下叠一层层薄片数据。
完整代码如下：
## Recursive Solution
# Initialize the 2x1 parameter vector x (i.e., x_0).
x_k = 10 * np.random.rand(2,1)
#Initialize the 2x2 covaraince matrix (i.e. P_0). Off-diangonal elements should be zero.
P_k = 0.01 * np.random.rand(2,2)
#x_k = np.array([[0.2],[1]])
#P_k = np.array([[0.01,0.01],[0.2,0.03]])
# Our voltage measurement variance (denoted by R, don't confuse with resistance).
R_k = np.array([[0.0225]])
# Pre allocate space to save our estimates at every step.
num_meas = I.shape[0]
x_hist = np.zeros((num_meas + 1, 2))
P_hist = np.zeros((num_meas + 1, 2, 2))
x_hist[0] = x_k.T
P_hist[0] = P_k
# Iterate over all the available measurements.
for k in range(num_meas):
# Construct H_k (Jacobian).
H_k = np.ones((num_meas, 2))
# Construct K_k (gain matrix).
K_k = P_hist[k,:,:].dot(H_k.T).dot(np.linalg.pinv(H_k.dot(P_hist[k,:,:]).dot(H_k.T) + R_k))
# Update our estimate.
x_k = x_k + K_k.dot( V/I - H_k.dot(x_k))
# Update our uncertainty (covariance)
P_k = (1 - K_k.dot(H_k)).dot(P_hist[k,:,:])
# Keep track of our history.
P_hist[k + 1,:,:] = P_k
x_hist[k + 1] = x_k.T
print('The slope and offset parameters of the best-fit line (i.e., the resistance and offset) are [R, b]:')
print(x_k[0, 0])
print(x_k[1, 0])那么我们展示一下结果：
plt.scatter(I, V, label='Data')
plt.plot(I_line, V_line, label='Batch Solution')
plt.xlabel('Current (A)')
plt.ylabel('Voltage (V)')
plt.grid(True)
I_line = np.arange(0, 0.8, 0.1).reshape(8, 1)
for k in range(num_meas):
V_line = x_hist[k, 0]*I_line + x_hist[k, 1]
plt.plot(I_line, V_line, label='Measurement {}'.format(k))
plt.legend()
plt.show()由于只有5次测量，不一定能到达最佳的结果，因此我们可以尝试多次初始化X和P值。
可以明显的看出，随着真实数据的持续输入，测量值越来越接近最佳的结果。 因此也称为Recursive Solution（递归解法）
