Kalman Filter

最新推荐文章于 2024-03-19 00:57:00 发布

路途…

最新推荐文章于 2024-03-19 00:57:00 发布

阅读量1.6k

点赞数

视频教程：https://www.youtube.com/watch?v=2-lu3GNbXM8

学习于：https://blog.csdn.net/tiandijun/article/details/72469471

尔曼滤波器的介绍（Introduction to the Kalman Filter）

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！
下面就要言归正传，讨论真正工程系统上的卡尔曼。

3．卡尔曼滤波器算法（The Kalman Filter Algorithm）

在这一部分，我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述，会涉及一些基本的概念知识，包括概率（Probability），随即变量（Random Variable），高斯或正态分配（Gaussian Distribution）还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述，我们结合下面PPT截图进行说明：

上两式子中，x(k)是k时刻的系统状态，u(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。y(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。q(k)和r(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。先给出KF算法的流程和五个核心更新方程如下：

KF算法

五个更新方程为：

1. 计算预测值；

2. 计算预测值和真实值之间误差的协方差矩阵；

3. 计算卡尔曼增益；

4. 根据预测值和测量值，计算估计值（滤波值）；

5. 计算估计值和真实值之间误差的协方差矩阵。

编写公式不方便，所以写成了PDF然后做了截图粘在了下面，下面就上面的例子和五个核心的公式对Kalman算法进行下说明：

就这样，算法就可以自回归的运算下去。

看到这聪明的同学可能已经看出来了，问道卡尔曼增益为什么会是第三步中那样求，现在只大致说一下原理，具体推到比较复杂，有兴趣的同学可以参考这文献去推一推。

还记得前面我们说的误差协方差矩阵$P_k$么，即求第k次最优温度的误差协方差矩阵，对应于上例中的3和2.35....这些值。看下面PPT，我们最小化P即可得到卡尔曼增益K，对应上例求解K只最小化最优温度值的偏差，即最小化P(K)：

// 一维滤波器信息结构体
typedef  struct{
	double filterValue;  //k-1时刻的滤波值，即是k-1时刻的值
	double kalmanGain;   //   Kalamn增益
	double A;   // x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)   x(k)是k时刻的系统状态 u(k)是k时刻对系统 
                //的控制量
    //H是测量系统的参数，对于多测量系统，H为矩阵
	double H;   // z(n)=H*x(n)+w(n),w(n)~N(0,R)  z(k)是k时刻的测量值
	double Q;   //预测过程噪声偏差的方差
	double R;   //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)
	double P;   //估计误差协方差
}  KalmanInfo;
/**
* @brief Init_KalmanInfo   初始化滤波器的初始值
* @param info  滤波器指针
* @param Q 预测噪声方差 由系统外部测定给定
* @param R 测量噪声方差 由系统外部测定给定
*/
void Init_KalmanInfo(KalmanInfo* info, double Q, double R)
{
	info->A = 1;  //标量卡尔曼   ?设置其他参数会怎样
	info->H = 1;  //
	info->P = 10;  //后验状态估计值误差的方差的初始值（不要为0问题不大）
	info->Q = Q;    //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出
	info->R = R;    //测量（观测）噪声方差 可以通过实验手段获得
	info->filterValue = 0;// 测量的初始值
}
double KalmanFilter(KalmanInfo* kalmanInfo, double lastMeasurement)
{
	//预测下一时刻的值
	double predictValue = kalmanInfo->A* kalmanInfo->filterValue;   //x的先验估计由上一个时间点的后验估计值和输入信息给出，此处需要根据基站高度做一个修改
	
	//求协方差
	kalmanInfo->P = kalmanInfo->A*kalmanInfo->A*kalmanInfo->P + kalmanInfo->Q;  //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q
	double preValue = kalmanInfo->filterValue;  //记录上次实际坐标的值
 
	//计算kalman增益
	kalmanInfo->kalmanGain = kalmanInfo->P*kalmanInfo->H / (kalmanInfo->P*kalmanInfo->H*kalmanInfo->H + kalmanInfo->R);  //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
	//修正结果，即计算滤波值
	kalmanInfo->filterValue = predictValue + (lastMeasurement - predictValue)*kalmanInfo->kalmanGain;  //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出  X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
	//更新后验估计
	kalmanInfo->P = (1 - kalmanInfo->kalmanGain*kalmanInfo->H)*kalmanInfo->P;//计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]
 
	return  kalmanInfo->filterValue;
}

python

参考http://pykalman.github.io/

https://github.com/pykalman/pykalman/blob/master/examples/standard/plot_em.py

r'''
==================================
Kalman Filter tracking a sine wave
==================================
This example shows how to use the Kalman Filter for state estimation.
In this example, we generate a fake target trajectory using a sine wave.
Instead of observing those positions exactly, we observe the position plus some
random noise.  We then use a Kalman Filter to estimate the velocity of the
system as well.
The figure drawn illustrates the observations, and the position and velocity
estimates predicted by the Kalman Smoother.
'''
import numpy as np
import pylab as pl

from pykalman import KalmanFilter

rnd = np.random.RandomState(0)

# generate a noisy sine wave to act as our fake observations
n_timesteps = 100
x = np.linspace(0, 3 * np.pi, n_timesteps)
observations = 20 * (np.sin(x) + 0.5 * rnd.randn(n_timesteps))

# create a Kalman Filter by hinting at the size of the state and observation
# space.  If you already have good guesses for the initial parameters, put them
# in here.  The Kalman Filter will try to learn the values of all variables.
# kf = KalmanFilter(initial_state_mean=0, n_dim_obs=2)        specify the size of the state and observation space.
kf = KalmanFilter(transition_matrices=np.array([[1, 1], [0, 1]]),
                  transition_covariance=0.01 * np.eye(2))                                                  

# You can use the Kalman Filter immediately without fitting, but its estimates
# may not be as good as if you fit first.
states_pred = kf.em(observations).smooth(observations)[0]
print(states_pred)
print('fitted model: {0}'.format(kf))

# Plot lines for the observations without noise, the estimated position of the
# target before fitting, and the estimated position after fitting.
pl.figure(figsize=(16, 6))
obs_scatter = pl.scatter(x, observations, marker='x', color='b',
                         label='observations')
position_line = pl.plot(x, states_pred[:, 0],
                        linestyle='-', marker='o', color='r',
                        label='position est.')
velocity_line = pl.plot(x, states_pred[:, 1],
                        linestyle='-', marker='o', color='g',
                        label='velocity est.')
pl.legend(loc='lower right')
pl.xlim(xmin=0, xmax=x.max())
pl.xlabel('time')
pl.show()

'''
===========================================
Using the Kalman Filter and Kalman Smoother
===========================================
This simple example shows how one may apply the Kalman Filter and Kalman
Smoother to some randomly generated data.
The Kalman Filter and Kalman Smoother are two algorithms for predicting the
hidden state of Linear-Gaussian system. In this script, all model parameters
are specified beforehand, so there is no need to fit the Kalman Filter's
parameters to the measurements. However, this is not essential; sensible
defaults will be used for unspecified parameters, and they may be learned using
:func:`KalmanFilter.em`.
The figure drawn shows the true, hidden state, the state estimates given by the
Kalman Filter, and the state estimates given by the Kalman Smoother.
'''
import numpy as np
import pylab as pl
from pykalman import KalmanFilter

# specify parameters
random_state = np.random.RandomState(0)
transition_matrix = [[1, 0.1], [0, 1]]
transition_offset = [-0.1, 0.1]
observation_matrix = np.eye(2) + random_state.randn(2, 2) * 0.1
observation_offset = [1.0, -1.0]
transition_covariance = np.eye(2)
observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1
initial_state_mean = [5, -5]
initial_state_covariance = [[1, 0.1], [-0.1, 1]]

# sample from model
kf = KalmanFilter(
    transition_matrix, observation_matrix, transition_covariance,
    observation_covariance, transition_offset, observation_offset,
    initial_state_mean, initial_state_covariance,
    random_state=random_state
)
states, observations = kf.sample(
    n_timesteps=50,
    initial_state=initial_state_mean
)
print(states)
print("obserbations")
print(observations)
# estimate state with filtering and smoothing
filtered_state_estimates = kf.filter(observations)[0]
smoothed_state_estimates = kf.smooth(observations)[0]

# draw estimates
pl.figure()
lines_observations=pl.plot(observations,color='y')
lines_true = pl.plot(states, color='b')
lines_filt = pl.plot(filtered_state_estimates, color='r')
lines_smooth = pl.plot(smoothed_state_estimates, color='g')
pl.legend((lines_observations[0],lines_true[0], lines_filt[0], lines_smooth[0]),
          ('obser','true', 'filt', 'smooth'),
          loc='lower right'
)
pl.show()

[[ 5.00000000e+00 -5.00000000e+00]
 [ 4.54404357e+00 -3.44572649e+00]
 [ 4.54333415e+00 -3.01205217e+00]
 [ 4.45519664e+00 -3.76614790e+00]
 [ 4.84301805e+00 -4.40831293e+00]
 [ 4.34794527e+00 -4.49549678e+00]
 [ 3.95334302e+00 -4.01733426e+00]
 [ 3.10369745e+00 -3.76098529e+00]
 [ 2.24027210e+00 -3.96328804e+00]
 [ 3.76731056e-02 -1.91251264e+00]
 [-1.50637352e+00 -1.03502229e+00]
 [-2.60534231e+00 -5.48119788e-01]
 [-2.78833652e+00 -1.97879178e-02]
 [-3.52463740e+00 -2.82529084e-01]
 [-4.46603659e+00 -1.90881169e+00]
 [-6.38711611e+00 -1.34602943e+00]
 [-5.89262849e+00 -1.11704652e+00]
 [-5.70199150e+00 -1.70185661e+00]
 [-6.28372969e+00 -1.54569127e+00]
 [-6.07263638e+00 -2.98193495e+00]
 [-5.29205030e+00 -3.06185979e+00]
 [-6.10141323e+00 -1.73941472e+00]
 [-6.01898830e+00 -9.32841552e-01]
 [-6.08536037e+00 -4.30852189e-01]
 [-7.49893058e+00  6.38544520e-01]
 [-7.94869511e+00 -8.91029191e-03]
 [-6.18202718e+00  9.97134366e-01]
 [-6.45031712e+00  1.89959076e+00]
 [-5.74627867e+00  2.92179743e+00]
 [-5.25586075e+00  4.34818333e+00]
 [-5.35619597e+00  6.29744706e+00]
 [-5.59636734e+00  6.93669625e+00]
 [-5.63854379e+00  7.71312955e+00]
 [-4.57122413e+00  6.72006804e+00]
 [-3.83254383e+00  7.45509947e+00]
 [-4.09985610e+00  8.67211576e+00]
 [-3.40088613e+00  1.04854585e+01]
 [-2.55079281e+00  9.92198020e+00]
 [-2.80606344e+00  9.58416015e+00]
 [-9.98226620e-01  9.77171139e+00]
 [-1.12127083e+00  8.32694030e+00]
 [ 5.32282025e-01  8.74566795e+00]
 [ 2.72605978e-01  9.52726247e+00]
 [ 6.69799721e-01  9.64474163e+00]
 [ 8.90655481e-01  7.52133847e+00]
 [ 4.38405989e-01  7.67350355e+00]
 [-1.87100565e-01  8.04055442e+00]
 [ 1.04023154e+00  7.96900809e+00]
 [ 3.90036830e+00  9.40553604e+00]
 [ 5.84058150e+00  1.01607998e+01]]
obserbations
[[  6.81526314  -6.22899083]
 [  5.38444676  -4.60994545]
 [  4.58543688  -4.36301003]
 [  8.90611562  -4.66686137]
 [  3.9733586   -7.24045806]
 [  4.33892607  -4.53262877]
 [  6.35718418  -7.54746635]
 [  3.22073444  -4.03730488]
 [  4.54773346  -7.1009213 ]
 [  1.50106391  -3.80086874]
 [  0.93472178  -2.71547629]
 [ -1.5899823   -3.12702465]
 [ -2.338171    -0.99299568]
 [ -2.44300239  -2.0853453 ]
 [ -4.54400033  -4.1628647 ]
 [ -5.57617782  -3.27166725]
 [ -7.28127843  -4.10721235]
 [ -4.85619314  -4.26468496]
 [ -5.13847136  -2.67714148]
 [ -7.789181    -3.27861974]
 [ -4.12762329  -4.2781678 ]
 [ -6.42483347  -2.74473111]
 [ -6.03900993  -0.95657565]
 [ -8.29690809  -3.3569111 ]
 [ -6.41908279   0.91293828]
 [-10.3720551   -0.21138057]
 [ -5.19716801   1.46479482]
 [ -7.55246585   0.59268051]
 [ -6.10904289   0.94308633]
 [ -4.25958436   3.62089162]
 [ -5.76115595   6.62651753]
 [ -4.56943265   6.93750252]
 [ -5.96356669   7.71487449]
 [ -2.46106895   7.13203319]
 [ -5.75987587   8.82127814]
 [ -2.06522889   8.68250456]
 [ -1.81140449  10.639988  ]
 [ -2.88642606   9.88575983]
 [ -1.27691745  12.34603904]
 [  1.59514075  11.63424637]
 [ -1.26465795   9.46406716]
 [  1.00925888   9.15860473]
 [  2.54263676   9.9593116 ]
 [  2.49002328   9.48621352]
 [  1.58607368   6.73749187]
 [  2.70709357   9.92742755]
 [  1.08524314   7.66164589]
 [  1.74317115   9.71747055]
 [  6.35502889  10.63665914]
 [  7.49781727  10.43892729]]

修改了如右图所示

# estimate state with filtering and smoothing
filtered_state_estimates = kf.em(observations).filter(observations)[0]
smoothed_state_estimates = kf.em(observations).smooth(observations)[0]

import cv2
import numpy as np
#创建一个大小800*800的空帧
frame = np.zeros((800,800,3),np.uint8)
#初始化测量坐标和鼠标运动预测的数组
last_measurement = current_measurement = np.array((2,1),np.float32)
 
last_predicition = current_prediction = np.zeros((2,1),np.float32)
'''
    mousemove()函数在这里的作用就是传递X,Y的坐标值，便于对轨迹进行卡尔曼滤波
'''
def mousemove(event,x,y,s,p):
    #定义全局变量
    global frame,current_measurement,measurements,last_measurement,current_prediction,last_prediction
    #初始化
    last_measurement = current_measurement
    last_prediction = current_prediction
    #传递当前测量坐标值
    current_measurement = np.array([[np.float32(x)],[np.float32(y)]])
    #用来修正卡尔曼滤波的预测结果
    kalman.correct(current_measurement)
    # 调用kalman这个类的predict方法得到状态的预测值矩阵，用来估算目标位置
    current_prediction = kalman.predict()
    #上一次测量值
    lmx,lmy = last_measurement[0],last_measurement[1]
    #当前测量值
    cmx,cmy = current_measurement[0],current_measurement[1]
    #上一次预测值
    lpx,lpy = last_prediction[0],last_prediction[1]
    #当前预测值
    cpx,cpy = current_prediction[0],current_prediction[1]
    #绘制测量值轨迹（绿色）
    cv2.line(frame,(lmx,lmy),(cmx,cmy),(0,100,0))
    #绘制预测值轨迹（红色）
    cv2.line(frame,(lpx,lpy),(cpx,cpy),(0,0,200))
 
cv2.namedWindow("kalman_tracker")
#调用函数处理鼠标事件，具体事件必须由回调函数的第一个参数来处理，该参数确定触发事件的类型（点击和移动）
'''
void setMousecallback(const string& winname, MouseCallback onMouse, void* userdata=0)
       winname:窗口的名字
       onMouse:鼠标响应函数，回调函数。指定窗口里每次鼠标时间发生的时候，被调用的函数指针。
                这个函数的原型应该为void on_Mouse(int event, int x, int y, int flags, void* param);
       userdate：传给回调函数的参数
 void on_Mouse(int event, int x, int y, int flags, void* param);
        event是 CV_EVENT_*变量之一
        x和y是鼠标指针在图像坐标系的坐标（不是窗口坐标系）
        flags是CV_EVENT_FLAG的组合， param是用户定义的传递到setMouseCallback函数调用的参数。
    常用的event：
        CV_EVENT_MOUSEMOVE
        CV_EVENT_LBUTTONDOWN
        CV_EVENT_RBUTTONDOWN
        CV_EVENT_LBUTTONUP
        CV_EVENT_RBUTTONUP
        和标志位flags有关的：
        CV_EVENT_FLAG_LBUTTON
'''
cv2.setMouseCallback("kalman_tracker",mousemove)
'''
Kalman这个类需要初始化下面变量：
转移矩阵，测量矩阵，控制向量(没有的话，就是0)，
过程噪声协方差矩阵，测量噪声协方差矩阵，
后验错误协方差矩阵，前一状态校正后的值，当前观察值。
    在此cv2.KalmanFilter(4,2)表示转移矩阵维度为4，测量矩阵维度为2
卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来，符合下式：
            X(k) = F(k) * X(k-1) + B(k)*U(k) + W（k）
其中
F(k)  是作用在xk−1上的状态变换模型（/矩阵/矢量）。 
B(k)  是作用在控制器向量uk上的输入－控制模型。 
W(k)  是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。
'''
kalman = cv2.KalmanFilter(4,2)
#设置测量矩阵
kalman.measurementMatrix = np.array([[1,0,0,0],[0,1,0,0]],np.float32)
#设置转移矩阵
kalman.transitionMatrix = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]],np.float32)
#设置过程噪声协方差矩阵
kalman.processNoiseCov = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]],np.float32)*0.03
 
while True:
    cv2.imshow("kalman_tracker",frame)
    if (cv2.waitKey(30) & 0xff) == 27:
        break
 
cv2.destroyAllWindows()