[五组数据]详解一个简单的卡尔曼滤波器python编程实例

程序员文章站 2024-03-25 22:03:04

...

上半年毕设的时候接触了卡尔曼滤波器，用matlab实现了该过程，尝试在一个课后作业中用三维度矩阵来存储变量的方式，结构似乎更好理解，记录一下分析的过程。可以查看https://blog.csdn.net/qwe900/article/details/105841154中的卡尔曼滤波器部分，有一些更详细的解读。

假如有一块电阻，你不知道它的阻值是多少，你想通过多次测量电压和电流值，从而用定义法求出来它的阻值大小，测量结果如下表所示：

Current (A)	Voltage (V)
0.2	1.23
0.3	1.38
0.4	2.06
0.5	2.47
0.6	3.17

但实际上，每次测量都有一些‘噪声’，因此修正的公式应该是这样????=????????+????

[五组数据]详解一个简单的卡尔曼滤波器python编程实例
如果在一张图片上显示这些坐标的话，

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

[五组数据]详解一个简单的卡尔曼滤波器python编程实例
如果采用最小二乘法的话，可以得到这样的图像。

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

[五组数据]详解一个简单的卡尔曼滤波器python编程实例
那么应用卡尔曼滤波器该如何操作呢？

思考一下矩阵的维度：

变量	维度
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])

[五组数据]详解一个简单的卡尔曼滤波器python编程实例
那么我们展示一下结果：

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