卡尔曼滤波器的python小案例实现（欢迎来找错）

泰伦斯-Ternence

已于 2024-04-24 00:10:49 修改

阅读量1k

点赞数 32

文章标签： python

于 2024-04-24 00:04:01 首次发布

本文链接：https://blog.csdn.net/qq_56883085/article/details/138142422

版权

卡尔曼滤波器的python小案例实现（欢迎来找错）

对于卡尔曼滤波器，网上有很多参考教程与推导，本项目主要是提供一个具体实现的案例，方便学习与参考

基本原理

预测

$\check{x}_k=A_k\hat{x}+u_k,\check{P}_k=A_k\hat{P}_{k-1}A_{k}^{T}+R$

更新，计算卡尔曼增益

$K=\check{P}_kC_{k}^{T}\left( C_k\check{P}_kC_{k}^{T}+Q_k \right) ^{-1}$

计算后验分布

$\hat{x}_k=\check{x}_k+K\left( z_k-C_k\check{x}_k \right)$
$\hat{P}_k=\left( I-KC_k \right) \check{P}_k$

问题背景（原创）

假设现在有一个点在有一个点在坐标（0，,0）处，它水平方向以v0的速度运动，竖直方向会以加速度为a的大小进行匀加速运动。因此这个轨迹实际上可以抽象为一个平抛运动。然后假设这个点上加装了一个陀螺仪，能够实时地测量出这个点的加速度，并且陀螺仪的精度也知道。在陀螺仪的每一个采样间隔时刻，假设能够实时获取卫星的经过解算后的坐标观测数据，精度也知道。如何用卡尔曼滤波进行优化呢？

alt text

基本方程（高中物理学过）

$\left\{ \begin{array}{l} Heng_k=Heng_{k-1}+v_0\varDelta t\\ Zong_k=Zong_{k-1}+\dot{Z}ong_{k-1}\varDelta t+\frac{1}{2}a_{k-1}^{'}\varDelta t^2\\ \dot{Z}ong_k=\dot{Z}ong_{k-1}+a_{k-1}^{'}\varDelta t\\ \end{array} \right.$
$\dot{Z}ong_k\text{表示}k\text{时刻}y\text{方向的速度}$
$a_{k-1}^{'}\text{表示加速度观测值}$

表示成矩阵形式

$\left[ \begin{array}{c} Heng_k\\ Zong_k\\ \dot{Z}ong_k\\ \end{array} \right] =\left[ \begin{matrix} 1& 0& 0\\ 0& 1& \varDelta t\\ 0& 0& 1\\ \end{matrix} \right] \left[ \begin{array}{c} Heng_{k-1}\\ Zong_{k-1}\\ \dot{Z}ong_{k-1}\\ \end{array} \right] +\left[ \begin{array}{c} v_0\varDelta t\\ \frac{1}{2}a_{k-1}^{'}\varDelta t^2\\ a_{k-1}^{'}\varDelta t\\ \end{array} \right]$

进一步推导

$\underset{x_k}{\underbrace{\left[ \begin{array}{c} Heng_k\\ Zong_k\\ \dot{Z}ong_k\\ \end{array} \right] }}=\underset{A_k}{\underbrace{\left[ \begin{matrix} 1& 0& 0\\ 0& 1& \varDelta t\\ 0& 0& 1\\ \end{matrix} \right] }}\underset{x_{k-1}}{\underbrace{\left[ \begin{array}{c} Heng_{k-1}\\ Zong_{k-1}\\ \dot{Z}ong_{k-1}\\ \end{array} \right] }}+\underset{u_k}{\underbrace{\left[ \begin{array}{c} v_0\varDelta t\\ \frac{1}{2}a_{k-1}\varDelta t^2\\ a_{k-1}\varDelta t\\ \end{array} \right] }}+\underset{w_k}{\underbrace{\left[ \begin{array}{c} 0\\ \frac{1}{2}w_k\varDelta t^2\\ w_k\varDelta t\\ \end{array} \right] }}$
$\text{其中：}a_{k-1}^{'}=a_{k-1}+w_k,\text{也就是真实值加误差噪声。}$

观测方程

$\underset{z_k}{\underbrace{\left[ \begin{array}{c} x\\ y\\ \end{array} \right] }}=\underset{C_k}{\underbrace{\left[ \begin{matrix} 1& 0& 0\\ 0& 1& 0\\ \end{matrix} \right] }}\underset{x_k}{\underbrace{\left[ \begin{array}{c} Heng_k\\ Zong_k\\ \dot{Z}ong_k\\ \end{array} \right] }}+\underset{v_k}{\underbrace{\left[ \begin{array}{c} v_{k}^{x}\\ v_{k}^{y}\\ \end{array} \right] }}$

求解

有关卡尔曼滤波的基本方程定义结束，接下来进行求解

import numpy as np
import matplotlib.pylab as plt
# 定义参数

# 水平初速度
v0=0.5

# 竖直加速度
a=10


start = 0   # 起始值
stop = 50   # 结束值（不包含在数组中）
# 采样间隔
delta_t=1

# 竖直加速度的误差
w_mean = 0    # 均值
w_std = 9   # 标准差
w_size = int((stop-start)/delta_t)  # 数组大小
w_noise = np.random.normal(w_mean, w_std, w_size)



# 构造GPS观测数据与加速度观测数据
a_obs=a+w_noise
# print(a_obs)


# 构造GPS观测值的误差
v_mean = 0    # 均值
v_std = 16   # 标准差
v_size = int((stop-start)/delta_t)  # 数组大小
v_noise = np.random.normal(v_mean, v_std, v_size)

x_yes=[]
x=[]
y=[]
i = 0
while i < 125:
    x_yes.append(i)
    x.append(i + v_noise[int(i/2.5)])
    y.append((a / (2 * v0 * v0)) * i * i + v_noise[int(i/2.5)])
    i += 2.5

print(np.array(x))
# print(np.array(y))

# 可视化GPS观测值
def f(v0,a,x):
    return a / (2 * v0 * v0) * x * x

x_yes=np.array(x_yes)
y_yes=f(v0,a,x_yes)

plot_x=np.array(x)
plot_y=np.array(y)

plt.plot(x_yes, y_yes)
plt.scatter(np.array(x), np.array(y), color='red', s=10,label='Scatter Points')
plt.xlabel('x')
plt.ylabel('y')
plt.title('正确地函数图像')
plt.grid(True)
plt.show()

[  5.69337282  -6.70358291   5.20005629  -3.1642016    8.96649194
   2.94264948  13.54474922   9.46044031   1.76099155  43.01931628
  47.01966072  28.63073915  16.40411915  20.11577761  39.86480856
  30.82327806  45.97475314  34.66940635  54.58517651  50.00387652
  47.91892513  60.73385031  54.91413202  39.44019536  68.19252353
  50.53582145  59.89798001  68.57587787  48.83605511  59.76311512
  73.09772855  98.94462715  91.60769251  82.04872022  99.62003629
  83.34378037  95.74242137  91.96367793  68.64679766  97.84523285
 117.93497059  98.53576392 102.31034303 103.28824097 116.82364354
 129.11815046 100.81599905 102.9071149  132.63592741 125.57352857]

png

# 接下来用卡尔曼滤波对位置进行优化

# 初始状态值
x0=[0,0,10]
x0=np.array(x0)

# 初始概率分布
P0=[1,1,1]
P0=np.diag(P0)

# 运动
uk=[v0*delta_t,0.5*a*delta_t*delta_t,a*delta_t]
uk=np.array(uk)

# 运动噪声
R=[0,(0.5*w_std*delta_t*delta_t)**2,(w_std*delta_t)**2]
R=np.array(R)

# 观测噪声
Q=[v_std**2,v_std**2]
Q=np.array(Q)

# 状态转移矩阵
A=np.eye(3)
A[1,2]=delta_t

# 观测矩阵
C=np.zeros((2,3))
C[0,0]=1
C[1,1]=1

# 状态优化后存在这儿
x_youhua=[]
y_youhua=[]

xk=x0
Pk=P0
x_youhua.append(xk[0])
y_youhua.append(xk[1])
for i in range(len(x)):
    # 预测
    xk_=A@np.transpose(xk)+np.transpose(uk)
 
    Pk_=A@Pk@np.transpose(A)+np.transpose(R)

    # 更新，计算卡尔曼增益
    K=Pk_@np.transpose(C)@np.linalg.inv(C@Pk_@np.transpose(C)+Q)

    # 计算后验
    zk=[x[i],y[i]]
    zk=np.array(zk) 
    xk=xk_+K@(np.transpose(zk)-C@xk_)
    x_youhua.append(xk[0]/100)
    y_youhua.append(xk[1])

    # 概率更新
    Pk=(np.eye(3)-K@C)@Pk_

print(x_youhua)
print(y_youhua)

plt.plot(x_yes, y_yes)
plt.scatter(np.array(x), np.array(y), color='red', s=10,label='Scatter Points')
plt.scatter(np.array(x_youhua), np.array(y_youhua), color='#aa00ff', s=10,label='Scatter Points')
plt.xlabel('x')
plt.ylabel('y')
plt.title('正确地函数图像')
plt.grid(True)
plt.show()

[0, -0.8977579557697737, 1.6366917806739805, 4.0683872184309156, 6.471353440096884, 8.848372211339216, 11.243391061201583, 13.622318979303191, 16.004463980666014, 18.38009272996062, 20.754373672088995, 23.124839128637976, 25.492014548445674, 27.85566216195411, 30.21615534091654, 32.573677300614335, 34.927860264959584, 37.27904673078576, 39.62688464129918, 41.971820001546774, 44.31352585939875, 46.65205000286674, 48.98761157880422, 51.319965679035604, 53.648992531164694, 55.9753144938602, 58.29830493412377, 60.618348370732605, 62.93544754264797, 65.24922560564583, 67.5601153192075, 69.86816137069368, 72.17354593455498, 74.47582818848407, 76.77499057889487, 79.07141409977498, 81.36465103840312, 83.65510253915193, 85.94256118532503, 88.22677530520652, 90.50846610229773, 92.78752111979469, 95.06341998162775, 97.33648660313979, 99.60669487306275, 101.87422445866503, 104.13906951115028, 106.4006985292198, 108.65952981102059, 110.9159427170216, 113.16945743229525]
[0, -89.77579557697733, 286.16917806739826, 901.8387218430917, 1764.6353440096887, 2874.837221133921, 4236.839106120158, 5847.231897930319, 7707.946398066602, 9818.009272996063, 12177.9373672089, 14787.483912863798, 17646.701454844566, 20755.56621619541, 24114.115534091656, 27722.36773006143, 31580.28602649596, 35687.90467307858, 40045.18846412991, 44652.18200015468, 49508.85258593987, 54615.205000286674, 59971.26115788042, 65576.99656790355, 71432.39925311647, 77537.53144938602, 83892.33049341237, 90496.83483707326, 97351.0447542648, 104454.92256056458, 111808.51153192075, 119411.81613706937, 127264.8545934555, 135367.5828188484, 143719.99905788948, 152322.14140997748, 161173.9651038403, 170275.51025391518, 179626.7561185325, 189227.67753052065, 199078.34661022975, 209178.75211197947, 219528.8419981628, 230128.64866031398, 240978.1694873063, 252077.4224458665, 263426.40695111506, 275025.069852922, 286873.45298110205, 298971.59427170217, 311319.4457432295]

png