目录html
理论部分不详细说明,网上大部分都给出很好的解释python
网上大部分都是理论和简单的例子,不多看到实战的信息git
本博文是笔者实际使用的总结,若有错误,请不吝指教github
对单个数据滤波,没法创建运动学模型算法
经过创建和自身相关的状态方程便可app
是一种平滑操做(上一时刻和当前时刻的关系)dom
举例:机器学习
对一个平面运动的质点进行跟踪(\(X、Y\))?ide
求解:学习
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的协方差矩阵进行比例的计算。。。。具体看文档
举例二:
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()
这部分比较简单,网上的例子大部分都是基于此的。。。
举例:
以汽车跟踪为例,目标是知道汽车时刻的状态\(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\)代码在这里
注意:
这里至关于创建了两个模型,一个线性模型,一个非线性模型,在不一样的时刻使用不一样的传感器进行更新
其实就是单个传感器合并到一块儿了。。。。
举例:
一个小车作不均则运动(速度、加速度、角速度等都是可变的),如今有两个传感器:仪器A和仪器B,他们都能测量 \(\omega\) 和 \(v\) ,那么如何进行融合两个传感器呢?
这里其实和Kalman的滤波比较相似,就是把两个传感器当作一个传感器的不一样时间序列 \(T_1,T_2\) 时刻测量的数据,而后滤波操做。
条件和Kalman多传感器融合B相同,单处理方式不一样
因为部分传感器精度不一样,进行特定的取舍颇有必要(亲身经历)
假设求取小车的 \(\omega\) 和 \(v\)
传感器A对\(\omega\) 测量较为准确
传感器B对 \(v\) 测量较为准确
解决:
其实咱们若是直接按照Kalman多传感器融合B进行操做的话,偏差基本不会缩小,可能还会增长
这个时候笔者的解决方案是把传感器A和B当作一个总体传感器C,传感器C测量的 \(\omega\) 是A的,测量的 \(v\) 是B的
那么咱们就把这个合起来的传感器C进行滤波就好了
实测可用。。。
看到网上不少人问这个问题,这里笔者没有亲自实现,只是作了猜测,不正确还望读者指正
解决:
因为卡尔曼只能一次融合两个信息(预测和观测),因此只能进行以下想法
注意:
笔者认为这种状况比较少见,由于 \(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\)也不一样
完毕!!!