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.

维纳过程模型：

退化模型：

$X(t)=\lambda t+\sigma B(t)$

其中， $\lambda$ 为drift coef， $\sigma$ 为diffusion coef ， $B(\cdot)$ 为标准布朗运动

状态空间方程：

$\begin{aligned}\lambda_i&=\lambda_{i-1}+\eta,\quad \eta\sim N(0,Q) \\x_i&=x_{i-1}+\lambda_{i-1}(t_i-t_{i-1})+\sigma\varepsilon_i, \quad \varepsilon_i\sim N(0,t_i-t_{i-1}) \end{aligned}$

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(t)
    print("n_samples:", n_samples)
    sigma2 = 2                           # difussion coef
    Q = 1                               # drift noise variation
    K = np.zeros(n_samples)
    P = np.ones([n_samples,n_samples])  # P0 = 1

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

    for i in np.arange(1, n_samples):
        # 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]
        y_pred[i] = y_pred[i-1] + λ_hat[i-1] * (t[i] - t[i-1])
        print(f"λ_hat：{λ_hat[i]}，P：{P[i][i]}")

    # y_pred[i + 1] = y_pred[i] + λ_hat[i]*(t[i]-t[i-1])


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

# data
t = df.iloc[0].index.to_numpy()
X = df.iloc[0].to_numpy()
kalman_filter_for_weiner_process(t,X)

数据如下：

t        X
0        0.0000
250      0.4741
500      0.9255
750      2.1147
1000     2.7168
1250     3.5110
1500     4.3415
1750     4.9076
2000     5.4782
2250     5.9925
2500     6.7224
2750     7.1303
3000     8.0006
3250     8.9193
3500     9.4940
3750     9.8675
4000    10.9446
Name: 0, dtype: float64

输出结果：

n_samples: 17
λ_hat：0.001895641743302679，P：0.000799680127948843
λ_hat：0.0018056719183482896，P：0.000799361022160161
λ_hat：0.004754442867440618，P：0.0007993610219565461
λ_hat：0.002410273837351531，P：0.0007993610219566571
λ_hat：0.003176187758265448，P：0.0007993610219566571
λ_hat：0.0033218835364738357，P：0.0007993610219566571
λ_hat：0.0022652446359513623，P：0.0007993610219566571
λ_hat：0.0022823862976238066，P：0.0007993610219566571
λ_hat：0.002057379861374825，P：0.0007993610219566571
λ_hat：0.002918911325328529，P：0.0007993610219566571
λ_hat：0.0016326282045899178，P：0.0007993610219566571
λ_hat：0.0034797235040138026，P：0.0007993610219566571
λ_hat：0.0036746441880028437，P：0.0007993610219566571
λ_hat：0.0022998989177841316，P：0.0007993610219566571
λ_hat：0.0014946436896421033，P：0.0007993610219566571
λ_hat：0.00430615258937267，P：0.0007993610219566571
y_pred: [0.         0.         0.47391044 0.92532842 2.11393913 2.71650759
 3.51055453 4.34102542 4.90733657 5.47793315 5.99227811 6.72200595
 7.130163   8.00009387 8.91875492 9.49372965 9.86739057]
λ_hat: [0.         0.00189564 0.00180567 0.00475444 0.00241027 0.00317619
 0.00332188 0.00226524 0.00228239 0.00205738 0.00291891 0.00163263
 0.00347972 0.00367464 0.0022999  0.00149464 0.00430615]
X: [ 0.      0.4741  0.9255  2.1147  2.7168  3.511   4.3415  4.9076  5.4782
  5.9925  6.7224  7.1303  8.0006  8.9193  9.494   9.8675 10.9446]

改进的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)
    print("n_samples:", n_samples)
    sigma2 = 20                           # difussion coef
    Q = 10                               # drift noise variation
    alpha, rho = 0.5, 0.5
    V = np.zeros(n_samples)
    gamma = np.zeros(n_samples)
    B = np.zeros(n_samples)
    C = np.zeros(n_samples)
    v = np.zeros(n_samples)
    

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

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

    for i in np.arange(1, n_samples):
        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]

        # pred
        y_pred[i] = y_pred[i-1] + λ_hat[i-1] * (t[i] - t[i-1])
        print(f"λ_hat：{λ_hat[i-1]}，P：{P[i][i]}")

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

# data
t = df.iloc[0].index.to_numpy()
X = df.iloc[0].to_numpy()
kalman_filter_for_weiner_process(t,X)

数据如下：

t        X
0        0.0000
250      0.4741
500      0.9255
750      2.1147
1000     2.7168
1250     3.5110
1500     4.3415
1750     4.9076
2000     5.4782
2250     5.9925
2500     6.7224
2750     7.1303
3000     8.0006
3250     8.9193
3500     9.4940
3750     9.8675
4000    10.9446
Name: 0, dtype: float64

输出结果：

n_samples: 17
λ_hat：0.0，P：0.0794223826714795
λ_hat：0.001882707581227437，P：0.07999541573485658
λ_hat：0.0018056044185198762，P：0.07999996363651007
λ_hat：0.004756798657830221，P：0.07999999821186066
λ_hat：0.002408400008471828，P：0.07999998331069946
λ_hat：0.0031767999998636902，P：0.07999999821186066
λ_hat：0.0033219999997595936，P：0.07999998331069946
λ_hat：0.002264400000187615，P：0.07999999821186066
λ_hat：0.0022823999999701966，P：0.07999998331069946
λ_hat：0.002057200000039947，P：0.07999999821186066
λ_hat：0.00291959999857214，P：0.07999998331069946
λ_hat：0.0016316000002284837，P：0.07999999821186066
λ_hat：0.003481199996937646，P：0.07999998331069946
λ_hat：0.003674799999965654，P：0.07999999821186066
λ_hat：0.0022988000022782225，P：0.07999998331069946
λ_hat：0.001494000000142767，P：0.07999999821186066
y_pred: [0.         0.         0.4706769  0.922078   2.11127766 2.71337767
 3.50757767 4.33807767 4.90417767 5.47477767 5.98907767 6.71897767
 7.12687767 7.99717767 8.91587767 9.49057767 9.86407767]
λ_hat: [0.         0.00188271 0.0018056  0.0047568  0.0024084  0.0031768
 0.003322   0.0022644  0.0022824  0.0020572  0.0029196  0.0016316
 0.0034812  0.0036748  0.0022988  0.001494   0.0043084 ]
X: [ 0.      0.4741  0.9255  2.1147  2.7168  3.511   4.3415  4.9076  5.4782
  5.9925  6.7224  7.1303  8.0006  8.9193  9.494   9.8675 10.9446]