本文共 5658 字,大约阅读时间需要 18 分钟。
友情提示不针对第三方,为了给读者更好的体验
理论部分不详细说明,网上大部分都给出很好的解释
网上大部分都是理论和简单的例子,很少看到实战的信息
本博文是笔者实际使用的总结,如有错误,请不吝指教
对单个数据滤波,无法建立运动学模型
通过建立和自身相关的状态方程即可
是一种平滑操作(上一时刻和当前时刻的关系)
举例:
对一个平面运动的质点进行跟踪( 、 X 、 Y 、X、Y 、X、Y)?
求解:
fig = plt.figure()axis = fig.add_subplot(1,1,1)func_data = lambda x : x + x^2z = 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 npimport matplotlib.pyplot as pltdef 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, Pdef 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) x=(px,py,vx,vy) x = ( p x , p y , v x , v y ) x=(p_x,p_y,v_x,v_y) x=(px,py,vx,vy)
已知的传感器有 、 l i d a r 、 r a d a r 、lidar、radar 、lidar、radar。
l i d a r lidar lidar:笛卡尔坐标系。可检测到位置,没有速度信息。其测量值 z = ( p x , p y ) z = ( p x , p y ) z=(px,py)z=(px,py) z=(px,py)z=(px,py)。
r a d a r radar radar:极坐标系。可检测到距离,角度,速度信息,但是精度较低。其测量值 z = ( ρ , ϕ , ρ ˙ ) z = ( ρ , ϕ , ρ ˙ ) z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙) z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙),
这是优达学城的一个例子,具体我也没视频网址。
m a t l a b matlab matlab代码地址在, p y t h o n python python代码在
注意:
这里相当于建立了两个模型,一个线性模型,一个非线性模型,在不同的时刻使用不同的传感器进行更新
其实就是单个传感器合并到一起了。。。。
举例:
一个小车做不均则运动(速度、加速度、角速度等都是可变的),现在有两个传感器:仪器A和仪器B,他们都能测量 ω \omega ω 和 v v v ,那么如何进行融合两个传感器呢?
这里其实和Kalman的滤波比较类似,就是把两个传感器当做一个传感器的不同时间序列 T 1 , T 2 T_1,T_2 T1,T2 时刻测量的数据,然后滤波操作。
条件和Kalman多传感器融合B相同,单处理方式不同
由于部分传感器精度不同,进行特定的取舍很有必要(亲身经历)
假设求取小车的 ω \omega ω 和 v v v
传感器A对 ω \omega ω 测量较为准确
传感器B对 v v v 测量较为准确
解决:
其实我们如果直接按照Kalman多传感器融合B进行操作的话,误差基本不会缩小,可能还会增加
这个时候笔者的解决方案是把传感器A和B当做一个整体传感器C,传感器C测量的 ω \omega ω 是A的,测量的 v v v 是B的
那么我们就把这个合起来的传感器C进行滤波就行了
实测可用。。。
看到网上很多人问这个问题,这里笔者没有亲自实现,只是做了猜想,不正确还望读者指正
解决:
由于卡尔曼只能一次融合两个信息(预测和观测),所以只能进行如下想法
注意:
笔者认为这种情况比较少见,因为 t t t 趋向于 ϵ \epsilon ϵ ,所以可以认为在无穷小的区间都近似于很恒定的
实在没办法的时候就使用EKF,原理都很简单,计算代价大许多
后续可以使用UKF进行操作,这部分笔者还未尝试
最后来个简单的总结,什么是卡尔曼 K K K?
两个相同信息:A 和 B
都满足 y = k x + b y=kx+b y=kx+b
那么如何得到 y y y ?
正常来说: y = ( A + B ) / 2 ∗ x + b y=(A+B)/2*x+b y=(A+B)/2∗x+b
但是好像不是非常好,这个 ( A + B ) / 2 (A+B)/2 (A+B)/2总是不变的,假如他们某个时刻占比改变了呢?
这个时候 K a l m a n Kalman Kalman的作用的体现了,他计算A和B的关系(看公式吧)
得出一个系数 K K K 这个 K K K 和A、B相关
此时: y = K ∗ x + b y=K*x+b y=K∗x+b
输入的A、B不同,那么 K K K也不同
完毕!!!
转载地址:http://avtgi.baihongyu.com/