kalman滤波python实现——基于维纳退化模型

参考文献：

Si X S, Wang W, Hu C H, et al. A Wiener-process-based degradation model with a recursive filter algorithm for remaining useful life estimation[J]. Mechanical Systems and Signal Processing, 2013, 35(1-2): 219-237.

维纳过程模型： 

• 退化模型：

其中，为drift coef，为diffusion coef ，为标准布朗运动

• 状态空间方程：

kalman滤波

• 算法细节：

• 代码实现 ：

import matplotlib.pyplot as plt
from math import pi
import numpy as np
from numpy import log, exp, sqrt
from numpy.random import normal, randn

def kalman_filter_for_weiner_process(t, X):
    ''''X is an array'''
    n_samples = len(X)
    _size = n_samples + 5
    sigma2 = 10                           # difussion coef
    Q = 10                                # drift noise variation
    K = np.zeros(_size)
    P = np.ones([_size,_size])  # P0 = 1

    λ_hat = np.zeros(_size) # a0 = 1
    y_pred = np.zeros(_size)

    for i in np.arange(0, n_samples):
        if i == 0:
            y_pred[i] = X[i]
        else:    
           # Prediction (expectation)
            P[i][i-1] = P[i-1][i-1] + Q 
            K[i] = (t[i] - t[i-1])**2 * P[i][i-1] + sigma2 * (t[i] - t[i-1]) # kalman 增益
            λ_hat[i] = λ_hat[i-1] + P[i][i-1] * (t[i] - t[i-1]) * (X[i] - X[i-1] - λ_hat[i-1] * (t[i] - t[i-1])) / K[i]

            # Correction (variance)
            P[i][i] = P[i][i-1] - P[i][i-1] * (t[i] - t[i-1])**2 / K[i] * P[i][i-1]

            # 基于更新后的λ_hat进行下一步的预测
            y_pred[i] = y_pred[i-1] + λ_hat[i] * (t[i] - t[i-1])
            
        print(f"λ_hat：{λ_hat[i]}，P：{P[i][i]}")

    plt.scatter(t,X, color = 'blue')
    plt.plot(t,X, color = 'blue', label = 'actual')
    plt.plot(t, y_pred[:len(t)], color = 'red', label='kalman')
    plt.scatter(t, y_pred[:len(t)], color = 'red')
    print(y_pred)
    print(λ_hat)
    print(X)
    plt.legend()
    

# data
t = np.arange(1,15)
X = np.sin(np.arange(1,15))  * 100
kalman_filter_for_weiner_process(t, X)

•  输出结果：​​​​​​​​​​​​​​

改进的kalman滤波

• 算法细节：

•  代码实现：

import matplotlib.pyplot as plt
from math import pi
import numpy as np
from numpy import log, exp, sqrt
from numpy.random import normal, randn

def strong_tracking_kalman_filter_for_weiner_process(t, X):
    ''''X is an array'''
    n_samples = len(t)
    _size = n_samples + 5 
    print("n_samples:", n_samples)
    sigma2 = 10                           # difussion coef
    Q = 10                               # drift noise variation
    alpha, rho = 0.5, 0.5
    V = np.zeros(_size)
    gamma = np.zeros(_size)
    B = np.zeros(_size)
    C = np.zeros(_size)
    v = np.zeros(_size)
    

    K = np.zeros(_size)
    P = np.ones([_size,_size])  # P0 = 1

    λ_hat = np.zeros(_size) # a0 = 0
    y_pred = np.zeros(_size)

    for i in np.arange(0, n_samples):
        if i == 0:
            y_pred[i] = X[i]
        else:    
            gamma[i] = X[i] - X[i-1] - λ_hat[i-1] * (t[i] - t[i-1])
            if i == 1:
                V[i] = gamma[i]**2
            elif i > 1:
                V[i] = (rho * V[i-1] + gamma[i]**2) / (1 + rho)
            B[i] = V[i] - Q * (t[i] - t[i-1])**2 - alpha * sigma2 * (t[i] - t[i-1])
            C[i] = P[i-1][i-1] * (t[i] - t[i-1])**2 - alpha * sigma2 * (t[i] - t[i-1])
            v0 = B[i] / C[i]
            if v0 >= 1:
                v[i] = v0
            else:
                v[i] = 1

            # Prediction (expectation)
            P[i][i-1] = v[i] * P[i-1][i-1] + Q 
            K[i] = (t[i] - t[i-1])**2 * P[i][i-1] + sigma2 * (t[i] - t[i-1]) # kalman 增益
            λ_hat[i] = λ_hat[i-1] + P[i][i-1] * (t[i] - t[i-1]) / K[i] * (X[i] - X[i-1] - λ_hat[i-1] * (t[i] - t[i-1])) 

            # Correction (variance)
            P[i][i] = P[i][i-1] - P[i][i-1] * (t[i] - t[i-1])**2 / K[i] * P[i][i-1]

            # 基于调整过后的λ_hat进行下一步的预测
            y_pred[i] = y_pred[i-1] + λ_hat[i] * (t[i] - t[i-1])

        print(f"λ_hat：{λ_hat[i-1]}，P：{P[i][i]}")

    plt.plot(t, X, color = 'blue', label = 'actual')
    plt.scatter(t, X, color = 'blue')
    plt.plot(t, y_pred[:len(t)], color = 'red', label='STF')
    plt.scatter(t, y_pred[:len(t)], color = 'red')
    print("y_pred:",y_pred)
    print("λ_hat:",λ_hat)
    print("X:",X)
    plt.legend()

# data
t = np.arange(1,15)
X = np.sin(np.arange(1,15))  * 100
strong_tracking_kalman_filter_for_weiner_process(t,X)

•  输出结果：

在实验的过程中发现，滤波的效果受到参数Q的取值的影响很大。