Kalman滤波的扩展——渐消Fading filter滤波python实现

原理：

在程序设计中，渐消因子的初值设为1。即：和1.7的表达式相同。

渐消滤波算例分析：

1. 常加速度模型（一维）

历元间隔1s；状态向量维度为3，分别是位置、速度、加速度；每个历元观测向量维度为2，分别是位置和速度。

滤波初值设置：

位置	0m
速度	5m/s
加速度	0m/s2
状态向量	[0,5,0]
状态转移矩阵	[[1,dt,0.5dt2],[0,1,dt],[0,0,1]]
设计矩阵	[[1,0,0], [0,1,0]]
状态向量协方差阵	单位阵（3*3）
观测噪声协方差阵	[[100,0], [0,0.01]]
过程噪声矩阵	[[dt5/20, dt4/8, dt3/6], [dt4/8, dt3/3, dt2/2], [dt3/6, dt2/2, dt]]

假设载体在X轴方向运动，0-200s载体以5m/s的速度匀速运动，200-400s载体以0.05m/s2匀加速运动，400-600s以-0.02 m/s2匀减速运动,600-800s匀速运动，800-1000s停止状态。直接在轨迹发生器的位置和速度输出上叠加白噪声信号来模拟X轴方向上的位置和速度。

标准Kalman滤波

渐消Kalman滤波

在动力学模型误差较小的情况下，标准Kalman滤波比渐消Kalman要好很多。如果错误地将动力学模型建立错误，会是怎样地结果？

假设动力学模型中状态转移矩阵变为：

[[1,dt,0.5dt2],[0,3,dt],[0,0,1]]

标准Kalman滤波（错误动力学模型）

渐消Kalman滤波（错误动力学模型）

可以看到，采用渐消滤波位置不发散了，位置误差在25m以内。

1. 假设载体进行匀加速运动，但设计动力学模型时误以为载体处于静止状态。

历元间隔1s；状态向量维度为1，位置；每个历元观测向量维度为1，位置。

滤波初值设置：

位置	0m
状态转移矩阵	[1]
设计矩阵	[1]
状态向量协方差阵	单位阵（1*1）
观测噪声协方差阵	[100]
过程噪声矩阵	[0.01]

0-400s载体以0.05m/s2进行匀加速运动。

标准Kalman滤波

渐消Kalman滤波

采用渐消Kalman滤波，误差大部分历元在20m以内，当动力学模型不准确的时候，采用渐消滤波可以有效抑制发散。

Python代码：

class FadingKalmanFilter(object):
    def __init__(self, alpha, dim_x, dim_z, dim_u=0):

        assert alpha >= 1
        assert dim_x > 0
        assert dim_z > 0
        assert dim_u >= 0

        self.alpha_ = alpha  # 渐消因子
        self.dim_x = dim_x  # 状态值个数
        self.dim_z = dim_z  # 观测值个数
        self.dim_u = dim_u  # 控制值个数

        self.x = zeros((dim_x, 1))  # Current state estimate 当前历元状态参数向量（dim_x*1）
        self.P = eye(dim_x)  # Current state covariance matrix 当前状态参数向量协方差阵（dim_x*dim_x）
        self.Q = eye(dim_x)  # Process noise covariance matrix 过程噪声协方差阵（dim_x*dim_x）
        self.B = 0.  # control transition matrix 控制矩阵（一般不考虑）
        self.F = np.eye(dim_x)  # state transition matrix  状态转移矩阵（dim_x*dim_x）
        self.H = zeros((dim_z, dim_x))  # Measurement function   观测设计矩阵（dim_z*dim_x）
        self.R = eye(dim_z)  # Measurement noise covariance matrix 观测噪声协方差矩阵（dim_z*dim_z）
        self.z = np.array([[None] * dim_z]).T

        # gain and residual are computed during the innovation step. We
        # save them so that in case you want to inspect them for various
        # purposes
        self.K = 0  # kalman gain 增益矩阵
        self.y = zeros((dim_z, 1))  # residual are computed during the innovation step 新息向量
        self.S = np.zeros((dim_z, dim_z))  # system uncertainty (measurement space)
        self.SI = np.zeros((dim_z, dim_z))  # inverse system uncertainty

        #self._alpha_sq = 1.  # fading memory control
        # identity matrix. Do not alter this.
        self._I = np.eye(dim_x)

        # Only computed only if requested via property 似然函数
        self._log_likelihood = log(sys.float_info.min)
        self._likelihood = sys.float_info.min
        self._mahalanobis = None

        # these will always be a copy of x,P after predict() is called
        self.x_prior = self.x.copy()  # 当前状态向量预测值
        self.P_prior = self.P.copy()  # 当前状态向量预测协方差阵

        # these will always be a copy of x,P after update() is called
        self.x_post = self.x.copy()  # 当前状态向量估值
        self.P_post = self.P.copy()  # 当前状态向量估值的协方差阵

        self.inv = np.linalg.inv

    def settingalpha_(self,z, u=None, B=None, F=None, Q=None,H=None,P=None,R=None):
        #计算预测状态向量
        if B is None:
            B = self.B
        if P is None:
            P = self.P
        if R is None:
            R = self.R
        if H is None:
            H = self.H
        if F is None:
            F = self.F
        if Q is None:
            Q = self.Q
        elif isscalar(Q):  # 如果Q是标量
            Q = eye(self.dim_x) * Q

        # x = Fx + Bu
        if B is not None and u is not None:
            x = dot(F, self.x) + dot(B, u)
        else:
            x = dot(F, self.x)
        #计算预测残差向量
        y= z - dot(H, self.x)
        #计算预测残差向量的协方差阵
        S=self.alpha_*dot(y,y.T)/(1+self.alpha_)

        HF=dot(H,F)
        HFP=dot(HF,P)
        M=dot(HFP,HF.T)
        HQ=dot(H,Q)
        N=S-dot(HQ,H.T)-R


        alpha=np.trace(N)/np.trace(M)

        if alpha>=1:
            self.alpha_=alpha
        else:
            self.alpha_=1.




        pass
    # 预测
    def predict(self, u=None, B=None, F=None, Q=None):
        """
        Predict next state (prior) using the Kalman filter state propagation
        equations.
        Parameters
        ----------
        u : np.array, default 0
            Optional control vector.控制向量（一般不用）

        B : np.array(dim_x, dim_u), or None
            Optional control transition matrix; a value of None
            will cause the filter to use `self.B`.

        F : np.array(dim_x, dim_x), or None
            Optional state transition matrix; a value of None
            will cause the filter to use `self.F`.

        Q : np.array(dim_x, dim_x), scalar, or None
            Optional process noise matrix; a value of None will cause the
            filter to use `self.Q`.
        """

        if B is None:
            B = self.B
        if F is None:
            F = self.F
        if Q is None:
            Q = self.Q
        elif isscalar(Q):  # 如果Q是标量
            Q = eye(self.dim_x) * Q

        # x = Fx + Bu
        if B is not None and u is not None:
            self.x = dot(F, self.x) + dot(B, u)
        else:
            self.x = dot(F, self.x)

        # P = FPF' + Q P变为状态预测向量的协方差矩阵
        self.P = self.alpha_ * dot(dot(F, self.P), F.T) + Q

        # save prior
        self.x_prior = self.x.copy()
        self.P_prior = self.P.copy()

    def update(self, z, R=None, H=None):
        """
        Add a new measurement (z) to the Kalman filter.
        If z is None, nothing is computed. However, x_post and P_post are
        updated with the prior (x_prior, P_prior), and self.z is set to None.
        Parameters
        ----------
        z : (dim_z, 1): array_like 当前历元观测向量
            measurement for this update. z can be a scalar if dim_z is 1,
            otherwise it must be convertible to a column vector.
            If you pass in a value of H, z must be a column vector the
            of the correct size.
        R : np.array, scalar, or None 测量噪声矩阵
            Optionally provide R to override the measurement noise for this
            one call, otherwise  self.R will be used.
        H : np.array, or None  设计矩阵
            Optionally provide H to override the measurement function for this
            one call, otherwise self.H will be used.
        """

        # set to None to force recompute
        self._log_likelihood = None
        self._likelihood = None
        self._mahalanobis = None

        if z is None:
            self.z = np.array([[None] * self.dim_z]).T
            self.x_post = self.x.copy()
            self.P_post = self.P.copy()
            self.y = zeros((self.dim_z, 1))
            return

        if R is None:
            R = self.R
        elif isscalar(R):
            R = eye(self.dim_z) * R

        if H is None:
            # z = reshape_z(z, self.dim_z, self.x.ndim)
            H = self.H

        # y = z - Hx 新息向量y
        # error (residual) between measurement and prediction
        self.y = z - dot(H, self.x)

        # common subexpression for speed
        PHT = dot(self.P, H.T)

        # S = HPH' + R
        # project system uncertainty into measurement space
        # S 新息向量y的协方差矩阵 R观测向量噪声的协方差矩阵
        self.S = dot(H, PHT) + R
        self.SI = self.inv(self.S)
        # K = PH'inv(S)
        # map system uncertainty into kalman gain
        # K 增益矩阵
        self.K = dot(PHT, self.SI)

        # x = x + Ky
        # 当前历元的状态向量估值
        # predict new x with residual scaled by the kalman gain
        self.x = self.x + dot(self.K, self.y)

        # P = (I-KH)P(I-KH)' + KRK'
        # This is more numerically stable
        # and works for non-optimal K vs the equation
        # P = (I-KH)P usually seen in the literature.
        # 状态向量估值的协方差矩阵
        I_KH = self._I - dot(self.K, H)
        self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T)

        # save measurement and posterior state
        self.z = deepcopy(z)
        self.x_post = self.x.copy()
        self.P_post = self.P.copy()