利用Python创建Kalman滤波器用于多普勒测速目标跟踪

利用Python构造Kalman滤波器和拓展卡尔曼滤波器Class用于目标跟踪

文章目录

- 利用Python构造Kalman滤波器和拓展卡尔曼滤波器Class用于目标跟踪

Kalman滤波概述

其基本思路是：

新的最佳估计基于原最佳估计和已知外部影响校正后得到的预测。
新的不确定性基于原不确定性和外部环境的不确定性得到的预测。

对于系统已知以下离散系统方程、线性观测方程：
$\begin{aligned} &x_{k+1}=\Phi_{k+1, k} x_{k}+\Psi_{k+1, k} u_{k}+\Gamma_{k} w_{k} \\ &z_{k+1}=H_{k+1} x_{k+1}+v_{k+1} \end{aligned}$ Kalman滤波的基本公式如下：
$\begin{aligned} \texttt{一步预测状态}\cdots \hat{x}_{k+1 \mid k}=&\Phi_{k+1, k} \hat{x}_{k \mid k}+\Psi_{k+1, k} u_{k} \\ \texttt{预测系统协方差矩阵}\cdots P_{k+1 \mid k}=&\Phi_{k+1, k} P_{k \mid k} \Phi_{k+1, k}^{\mathrm{T}}+\Gamma_{k} Q_{k} \Gamma_{k}^{\mathrm{T}} \\ \texttt{Kalman增益计算}\cdots K_{k+1}=&P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}\left[H_{k+1} P_{k+1 \mid k} H_{k+1}^{T}+R_{k+1}\right]^{-1} \\ \texttt{修正后的估计状态}\cdots \hat{x}_{k+1 \mid k+1}=&\hat{x}_{k+1 \mid k}+K_{k+1}\left[z_{k+1}-H_{k+1} \hat{x}_{k+1 \mid k}\right] \\ \texttt{状态误差最优估计}\cdots P_{k+1 \mid k+1}=&\left[I-K_{k+1} H_{k+1}\right] P_{k+1 \mid k} \end{aligned}$ 式中：

符号	解释
$\hat x_{k+1/k}$	根据 $k$ 时刻状态和系统方程预测的（先验）值
$\hat x_{k+1/k+1}$	Kalman滤波得到的 $k + 1$ 时刻，也就是综合了以前结果和当前这一步观测值，所得到的状态估计值
$x_{k+1}$ 或者 $x_{k+1/k+1}$	$k + 1$ 时刻真实的状态
$\hat z_{k+1/k}$	根据预测状态 $x_{k+1/k}$ ，对观测结果的预测
$z_{k+1}$	$k + 1$ 时刻传感器的测量值
$z_{k+1}-H\hat x_{k+1/k}$	预测的和实际测量值之间的差距
$P_{k+1/k}$	预测值和真实值之间误差的协方差矩阵
$P_{k+1/k+1}$	下一步 $k + 1$ 估计值和真实值之间的误差协方差矩阵
$K_k$	kalman增益

状态协方差矩阵定义为 $P_{k}=E\left[e_{k} e_{k}^{T}\right]=E\left[\left(x_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^{T}\right]$ ，代表估计值和真实值之间的误差。滤波时，可以估计出状态协方差的初值 $P_0$ （迭代更新），测量噪声协方差 $\operatorname{Cov}(w_1)$ 、系统噪声协方差矩阵 $=\operatorname{Cov}([v_1,v_2])$ ，以及初值 $\hat{x}_{0 \mid 0}=E x_{0}=m_{0}$

关于这几个矩阵的设定，可见过客冲冲 - 理解Kalman滤波的使用 cnblogs。

P是误差矩阵，初始化可以是一个随机的矩阵或者0，只要经过几次的处理基本上就能调整到正常的水平，因此也就只会影响前面几次的滤波结果。
Q和R分别是预测和观测状态协方差矩阵，一般可以简单认为系统状态各维之间(即上面的a和b)相互独立，那么Q和R就可以设置为对角阵。而这两个对角线元素的大小将直接影响着滤波结果，若Q的元素远大于R的元素，则预测噪声大，从而更相信观测值，这样可能使得kalman滤波结果与观测值基本一致；反之，则更相信预测，kalman滤波结果会表现得比较规整和平滑；若二者接近，则滤波结果介于前面两者之间，根据实验效果看也缺乏实际使用价值。

Kalman滤波器Python类

下面这个是根据上图中的公式写的，简单改一下就是EKF，参考360doc 时钟转动 / 卡尔曼滤波 / Kalman滤波器的C++实现

import numpy as np
import math
# import scipy.stats as st
# import scipy
# import matplotlib as mpl
import matplotlib.pyplot as plt

class KalmanFilter:
    stateSize = 1
    measureSize = 1
    controlSize = 0
    IsNonLinearObserve , funHz = (0, '')
    x,z,A,B,G,u,Px,K,Hz,Qv,Rm = [0,0,0,0,0,0, 0,0,0,0,0]
    
    def __init__(self,n,m,nc):
        self.stateSize = n
        self.measureSize = m
        self.controlSize = nc
        self.x = np.zeros([n,1])
        self.z = np.zeros([m,1])
        
        self.In = np.eye(n)
        self.A = np.eye(n)         # state transition matrix, discrete
        if nc>0:
            self.u = np.zeros([nc,1])
            self.B = np.zeros(n,nc)
        
        self.G = np.eye(n)
        self.Px = np.eye(n)*1e-6
        self.Qv = 1e-6*np.eye(n)  # process variance
        self.K  = np.zeros([n,m]) # kalman gain
        self.Hz = np.zeros([m,n])
        self.Rm = 1e-6*np.eye(m)  # estimate of measurement variance,
    
    def init(self,x_,P_,Q_,R_):
        for i in range(0,self.stateSize):
            self.x[i,:] = x_[i,:]
        self.Px = P_
        self.Qv = Q_
        self.Rm = R_
        return self
    
    def predict(self,A_):
        self.A = A_
        self.x = self.A@self.x
#         print(self.x)
        self.P = self.A@self.Px@self.A.T + self.G@self.Qv@self.G.T 
        return self

    def observe(self,x):
        if self.IsNonLinearObserve:# 这是非线性观测方程
            z = self.funHz(x)
            z_predict = np.array( [ [z[0]], [z[1]] ] )
        else:
            z_predict = self.Hz@x

        return z_predict

    def update(self,H_,z_):
        self.Hz = H_
        Ht = self.Hz.T
        temp1 = self.Hz@self.P@Ht + self.Rm
#         temp2 = temp1.inverse() #(H*P*H'+R)^(-1)
        self.K = self.P@Ht@np.linalg.inv(temp1)

        self.z = self.observe(self.x)# 根据方程类型调用不同的观测函数

        self.x = self.x + self.K@(z_ - self.z)
#         print(self.x)
        self.P = (self.In - self.K@self.Hz)@self.Px
        return self

线性动力学、线性观测

以下是一个一维Doppler测速估计目标状态的模型：
$\begin{aligned} &\dot{x}=Ax+\nu =\left[\begin{array}{l} \dot{r} \\ \dot{v} \end{array}\right]=\left[\begin{array}{ll} 0 & 1 \\ 0 & 0 \end{array}\right]\left[\begin{array}{l} r \\ v \end{array}\right]+\left[\begin{array}{ll} v_{1}\\ v_{2} \end{array}\right] \\ &z = Hx +w=\left[\begin{array}{ll} 0 & 1 \end{array}\right]\left[\begin{array}{r} r \\ v \end{array}\right]+w_1 \end{aligned}$ 然后协方差矩阵定义为： $\operatorname{Cov}(\hat{x}-x)=P \ ,\ \operatorname{Cov}([v_1,v_2])=Q \ ,\ \operatorname{Cov}(w_1)=R$ 目标初始时刻 $\approx 0$ , 已知目标是匀速直线运动，要求用KF估计其位置和速度。

初始化动力学系统：

# # 
# SEC 2, SYSTEM# 
# # 
def calExpAt(A,dt):
    n = len(A)
    I = np.eye(n)
    return I+A*dt+A@A/2*dt*dt

#########################################################################
# 系统设置，初值估计
#########################################################################
measureStep = 0.1;
tf = 30;
lth = 1 + int(tf/measureStep);
ts = np.linspace(0,tf,lth)

A = np.array([[0,1.0],[0,0]])           # system matrix
H = np.array([[0,1.0]])                 # 注意size

x0 = np.array([0,0.8])                    # 我这里随意设置了一个初值，与真实值相差较大
x0 = x0[:,np.newaxis]
# x0 = np.array([[x1],[x2]]) 
#########################################################################
# 先验统计误差，
#########################################################################
sigmaQv = np.array([0.1,0.1])                     # estimated pertubation of dynamical system per step
                                        # 如果第一步的初始状态估计误差就很大，这里就需要设置稍大；否则，收敛将很慢
Q = np.eye(2)
Q[0,0] = Q[0,0]*sigmaQv[0]**2           # pertubation from white noise, may be from 
Q[1,1] = Q[1,1]*sigmaQv[1]**2
sigmaRm = 5                           # a priori precision of measurement
R = np.array([[sigmaRm**2]])            # covariance of measurement precision

P0 = np.eye(2)                          # error of state, estimated at start
P0[0,0] = 1*P0[0,0]
P0[1,1] = 3*P0[1,1]                                

#########################################################################
# 采用先验误差设定，模拟真实的、带有噪声的测量数据
#########################################################################
# z_ts = np.linspace(0,18,lth)+np.random.normal(0,sigmaRm,lth)  # 匀加速
v = 1.2
z_real = v*np.ones((lth))               # 一维数组，尚未扩充维度到2
z_ts = z_real+np.random.normal(0,sigmaQv[1]*sigmaQv[1],lth)
z_ts = z_ts[np.newaxis, :]              # 作为行向量输入

创建Kalman滤波器并滤波，获得目标的状态：


# # 
# SEC 3, ESTIMATE/# 
# # 
# 创建并初始化
dopp = KalmanFilter(2,1,0)
# 设置先验误差矩阵
dopp.init(x0,P0,Q,R)

# 保存数据
# P_ts = P[:,:,np.newaxis]
x_ts = np.zeros([lth,2])

for it in range(0,lth):
    dopp.predict(calExpAt(A,measureStep))
    dopp.update(H,z_ts[:,it,np.newaxis])
    x_ts[it,:] = dopp.x.T[0]
    # x_ts[0,it] = dopp.x[0],x_ts[1,it] = dopp.x[1]
    
# 绘图
plt.subplot(2,1,1)
plt.plot(ts,x_ts[:,1])
plt.plot(ts,z_ts[0])
plt.legend(['filtered data', 'measure data'])
plt.title('Doppler velocity estimate result')

plt.subplot(2,1,2)
plt.plot(ts,x_ts[:,1]-z_real)
plt.xlabel('t/[s]')
plt.title('estimatation error')
plt.show()

以下是两个设定的例子, 估计速度初值为 $0.8 m / s$ ，实际速度 $1.2 m / s$ ；初始位置为 $0$ ，协方差矩阵设定不同：
$Q=\operatorname{diag}(0.1,0.1)^2,R=\operatorname{diag}(5.0)^2,P_0=\operatorname{diag}(3,3)$ 请添加图片描述
$Q=\operatorname{diag}(1.0,0.1)^2,R=\operatorname{diag}(0.2)^2,P_0=\operatorname{diag}(3,3)$

按照误差标准差公式 $S=\sqrt{\frac{\sum_{i=1}^{n}\left(x_{i}-\bar{x}\right)^{2}}{n-1}}$ 统计误差

# 统计一下估计误差
v_hat = x_ts[-100:,1]
err = v_hat-z_real[-100:]
std = np.sum((err- np.mean(err))**2)/99
print('estimated covariance of navigation: ',math.sqrt(std))

所有代码

限于篇幅，这里给出剩下的内容，包括一个非线性观测、非线性系统情况下的拓展卡尔曼滤波案例。
Simple Extended Kalman Filter for target tracking