目录
@
理论部分不详细说明,网上大部分都给出很好的解释
网上大部分都是理论和简单的例子,很少看到实战的信息
本博文是笔者实际使用的总结,如有错误,请不吝指教
Kalman理论介绍
一. 简单理论介绍理论
二. 升华理论介绍
- [x] 优达学城的笔记
- [x] 徐亦达机器学习Kalman Filter 卡尔曼滤波
- [x] 无人驾驶大神笔记EKF
- [x] 英文文献一
- [x] 英文文献二
- [x] 车辆运动导航算法
- [x] 很多文献忘记记录,有侵权请告知
Kalman基本应用
一. Kalman跟踪/滤波
对单个数据滤波,无法建立运动学模型
通过建立和自身相关的状态方程即可
是一种平滑操作(上一时刻和当前时刻的关系)
举例:
对一个平面运动的质点进行跟踪(\(、X、Y\))?
- 速度$、,v、\alpha,\omega $都是未知状态
求解:
fig = plt.figure()
axis = fig.add_subplot(1,1,1)
func_data = lambda x : x + x^2
z = np.mat(func_data(np.arange(1,100)))
x_mat = np.mat([[0,],[0.]])#状态矩阵[x,delta_x]
p_mat = np.mat([[1, 0], [0, 1]])#状态协方差矩阵
f_mat = np.mat([[1, 1],[0.,1.]])#状态转移矩阵
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
h_mat = np.mat([1.,0])# 观测矩阵[x]
r_mat = np.mat([1])#观测协方差矩阵
result = []
for i in range(z.shape[1]):
x_predict = f_mat * x_mat
p_predict = f_mat * p_mat * f_mat.T + q_mat
kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
x_mat = x_predict + kalman *(z[0, i] - h_mat * x_predict)
p_mat = (np.eye(2) - kalman * h_mat) * p_predict
result.append(x_predict[0,0])
axis.plot(result,label='predict')
axis.plot(z.tolist()[0],label='groundtruth')
axis.legend()
二. Kalman预测/融合(单传感器)
- [x] 运动学模型
- [x] 单一传感器
- [x] 速度$、,v、\alpha,\omega $推导可知
举例一:
一个运动小车的位置和速度的测量等信息可以被测量(一个传感器),也可以通过牛顿运动学方程进行解算,这两个到底谁占的比例高?使用Kalman的协方差矩阵进行比例的计算。。。。具体看文档
举例二:
import numpy as np
import matplotlib.pyplot as plt
def kalman_xy(x, P, measurement, R,
motion = np.matrix('0. 0. 0. 0.').T,
Q = np.matrix(np.eye(4))):
"""
Parameters:
x: initial state 4-tuple of location and velocity: (x0, x1, x0_dot, x1_dot)
P: initial uncertainty convariance matrix
measurement: observed position
R: measurement noise
motion: external motion added to state vector x
Q: motion noise (same shape as P)
"""
return kalman(x, P, measurement, R, motion, Q,
F = np.matrix('''
1. 0. 1. 0.;
0. 1. 0. 1.;
0. 0. 1. 0.;
0. 0. 0. 1.
'''),
H = np.matrix('''
1. 0. 0. 0.;
0. 1. 0. 0.'''))
def kalman(x, P, measurement, R, motion, Q, F, H):
'''
Parameters:
x: initial state
P: initial uncertainty convariance matrix
measurement: observed position (same shape as H*x)
R: measurement noise (same shape as H)
motion: external motion added to state vector x
Q: motion noise (same shape as P)
F: next state function: x_prime = F*x
H: measurement function: position = H*x
Return: the updated and predicted new values for (x, P)
See also http://en.wikipedia.org/wiki/Kalman_filter
This version of kalman can be applied to many different situations by
appropriately defining F and H
'''
# UPDATE x, P based on measurement m
# distance between measured and current position-belief
y = np.matrix(measurement).T - H * x
S = H * P * H.T + R # residual convariance
K = P * H.T * S.I # Kalman gain
x = x + K*y
I = np.matrix(np.eye(F.shape[0])) # identity matrix
P = (I - K*H)*P
# PREDICT x, P based on motion
x = F*x + motion
P = F*P*F.T + Q
return x, P
def demo_kalman_xy():
x = np.matrix('0. 0. 0. 0.').T
P = np.matrix(np.eye(4))*1000 # initial uncertainty
N = 20
true_x = np.linspace(0.0, 10.0, N)
true_y = true_x**2
observed_x = true_x + 0.05*np.random.random(N)*true_x
observed_y = true_y + 0.05*np.random.random(N)*true_y
plt.plot(observed_x, observed_y, 'ro')
result = []
R = 0.01**2
for meas in zip(observed_x, observed_y):
x, P = kalman_xy(x, P, meas, R)
result.append((x[:2]).tolist())
kalman_x, kalman_y = zip(*result)
plt.plot(kalman_x, kalman_y, 'g-')
plt.show()
demo_kalman_xy()
这部分比较简单,网上的例子大部分都是基于此的。。。
三. Kalman多传感器融合A
- [x] 运动学模型
- [x] 多个传感器
- [x] 传感器时间序列不同
举例:
以汽车跟踪为例,目标是知道汽车时刻的状态\(x=(p_x,p_y,v_x,v_y)\)\(x=(p_x,p_y,v_x,v_y)\)
已知的传感器有\(、lidar、radar\)。
\(lidar\):笛卡尔坐标系。可检测到位置,没有速度信息。其测量值\(z=(px,py)z=(px,py)\)。
\(radar\):极坐标系。可检测到距离,角度,速度信息,但是精度较低。其测量值\(z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙)\),
这是优达学城的一个例子,具体我也没视频网址。
\(matlab\)代码地址在这里,\(python\)代码在这里
注意:
这里相当于建立了两个模型,一个线性模型,一个非线性模型,在不同的时刻使用不同的传感器进行更新
其实就是单个传感器合并到一起了。。。。
四. Kalman多传感器融合B
- [ ] 无运动学模型
- [x] 多传感器
- [x] 传感器时序相同
举例:
一个小车做不均则运动(速度、加速度、角速度等都是可变的),现在有两个传感器:仪器A和仪器B,他们都能测量 \(\omega\) 和 \(v\) ,那么如何进行融合两个传感器呢?
- 具体的代码这里不方便给出,有需要可以一起讨论
这里其实和Kalman的滤波比较类似,就是把两个传感器当做一个传感器的不同时间序列 \(T_1,T_2\) 时刻测量的数据,然后滤波操作。
五. Kalman多传感器融合C
- [ ] 无运动学模型
- [x] 多传感器
- [x] 传感器时序相同
条件和Kalman多传感器融合B相同,单处理方式不同
由于部分传感器精度不同,进行特定的取舍很有必要(亲身经历)
假设求取小车的 \(\omega\) 和 \(v\)
传感器A对\(\omega\) 测量较为准确
传感器B对 \(v\) 测量较为准确
解决:
其实我们如果直接按照Kalman多传感器融合B进行操作的话,误差基本不会缩小,可能还会增加
这个时候笔者的解决方案是把传感器A和B当做一个整体传感器C,传感器C测量的 \(\omega\) 是A的,测量的 \(v\) 是B的
那么我们就把这个合起来的传感器C进行滤波就行了
实测可用。。。
六. Kalman多传感器融合D
- [x] 运动学模型
- [x] 多传感器
- [x] 传感器时序相同
看到网上很多人问这个问题,这里笔者没有亲自实现,只是做了猜想,不正确还望读者指正
解决:
由于卡尔曼只能一次融合两个信息(预测和观测),所以只能进行如下想法
- 进行两次融合,一次是预测和传感器A,一次结果和传感器B(这部分就是多传感器B)
- 进行一次融合,预测和新的传感器C(Kalman多传感器融合C)
七. Extend Kalman
- 运动学模型不是线性的
- 使用雅克比代替状态矩阵和观测矩阵
注意:
笔者认为这种情况比较少见,因为 \(t\) 趋向于 \(\epsilon\) ,所以可以认为在无穷小的区间都近似于很恒定的
实在没办法的时候就使用EKF,原理都很简单,计算代价大许多
后续可以使用UKF进行操作,这部分笔者还未尝试
本文总结
最后来个简单的总结,什么是卡尔曼\(K\)?
两个相同信息:A 和 B
都满足\(y=kx+b\)
那么如何得到 \(y\) ?
正常来说:\(y=(A+B)/2*x+b\)
但是好像不是非常好,这个\((A+B)/2\)总是不变的,假如他们某个时刻占比改变了呢?
这个时候\(Kalman\)的作用的体现了,他计算A和B的关系(看公式吧)
得出一个系数 \(K\) 这个\(K\) 和A、B相关
此时:\(y=K*x+b\)
输入的A、B不同,那么\(K\)也不同
完毕!!!