[PYTHON] Filtre de Kalman que vous pouvez comprendre

À propos de cet article

Implémentons le filtre de Kalman en utilisant la marche aléatoire comme exemple.

Qu'est-ce que le filtre Kalman?

C'est une technique pour supprimer le bruit du personnage nommé et estimer sa véritable apparence.

Théorique

Comme paramètre typique, considérez un système comme celui ci-dessous.

\begin{align}
x_{t+1} &= F_t x_t + G_t w_t \tag{1.1}\\
y_t &= H_t x_t + v_t \tag{1.2}\\
\end{align}

$ x_t $ représente l'état interne du système et $ y_t $ est son observation. L'état interne $ x_t $ évolue dans le temps selon $ (1.1) $. Par contre, la valeur observée $ y_t $ est balayée par du bruit selon $ (1.2) $. Le but est de déduire un état interne $ x_t $ qui ne peut pas être observé directement à partir de la valeur observée $ y_t $. Pour faciliter la configuration, supposons que $ w_t $ et $ v_t $ suivent des distributions gaussiennes indépendantes et ont une autocorrélation seulement en même temps. En utilisant l'indépendance de $ w_t $ et $ v_t $ et le théorème bayésien

\begin{align}
p(x_t|Y^t) &= \frac{p(y_t|x_t)p(x_t|Y^{t-1})}{p(y_t|Y^{t-1})} \tag{2.1}\\
p(x_{t+1}|Y^t) &= \int p(x_{t+1}|x_t)p(x_t|Y^t)dx_t \tag{2.2}
\end{align}

L'expression relationnelle est obtenue. Où $ Y ^ t $ est la valeur de $ y_t $ jusqu'au temps $ t $, c'est-à-dire

Y^t=\left\{y_1, y_2, ..., y_t\right\}

Supposer que Maintenant,(2.1)De tempstValeurs observées jusqu'àY^tDans l'état que je connaissaisx_tValeur estimée de\hat{x}\_{t|t}Il peut être obtenu. Mais,(2.1)En regardant le côté droit dep(x_t|Y^{t-1})Est inclus. pour cette raison,\hat{x}\_{t|t}Pour avoir le tempst-1Valeurs observées jusqu'àY\^{t-1}Dans l'état oùx_tValeur estimée de\hat{x}\_{t|t-1}Tu dois savoir. cette\hat{x}\_{t|t-1}Est(2.2)Vous pouvez en obtenir plus,(2.2)En regardant le côté droit de、\hat{x}\_{t|t-1}を得るにEstさらに\hat{x}\_{t-1|t-1}Vous pouvez voir que c'est nécessaire. De ce qui précède,\hat{x}\_{t|t-1}Quand\hat{x}\_{t|t}En calculant en alternancex\_tの推定を行うQuandいう流れになるこQuandが納得できるはずです。

En revanche, si les valeurs observées jusqu'à l'heure de fin sont obtenues

p(x_t|Y^N) = \int \frac{p(x_{t+1}|x_t)p(x_t|Y^t)p(x_{t+1}|Y^N)}{p(x_{t+1}|Y^t)}dx_{t+1} \tag{2.3}

Peut également être dérivé. ici,NC'est le temps de la fin.(2.3)De toutes les valeurs observéesY^NDans l'état oùx_tValeur estimée de\hat{x}\_{t|N}Il peut être obtenu. Cependant, la molécule du côté droit/En regardant le troisième élément,\hat{x}\_{t+1|N}Je sais que je dois le demander à l'avance. Donc,\hat{x}\_{t|N}Le processus de recherche est dans l'ordre inverse du tempst=NDet=1Continuez vers. Aussi,(2.3)La molécule sur le côté droit de/第2項Deは、\hat{x}\_{t|t}Puisque nous savons que nous devons également le savoir, nous évaluerons le temps dans le sens avant, puis dans le sens inverse. Ce processus d'aller dans la direction opposée s'appelle le lissage.

Expérimentez avec une marche aléatoire

Marche aléatoire

À titre d'exemple simple, considérons le cas d'une marche aléatoire. Les formules pour $ (1.1) $ et $ (1.2) $ sont

\begin{align}
x_{t+1} &= x_t + w_t \tag{3.1}\\
y_t &= x_t + v_t \tag{3.2}
\end{align}

Ce sera. Cependant, supposons que $ w_t $ et $ v_t $ suivent des distributions gaussiennes indépendantes et n'ont une autocorrélation qu'en même temps. Vous pouvez considérer $ (3.1) $ comme le vrai mouvement de la marche aléatoire et $ (3.2) $ comme le bruit ajouté pour obtenir les valeurs observées. Je ne connais que le $ y_t $ observé, mais je veux deviner le $ x_t $ original en supprimant le bruit.

Au fait, faisons une promenade aléatoire avec python. Les trois packages suivants sont suffisants.

import numpy as npimport numpy as np
import matplotlib.pyplot as plt
import seaborn as sns
sns.set_context('poster')

Nous mettrons en œuvre la formule ci-dessus. Tout d'abord, faites une marche aléatoire de $ (3.1) $, puis ajoutez du bruit comme $ (3.2) $.

def random_walker(start_position=0, mean=0, deviation=1, n_steps=99, seed=None):
    
    if seed is not None:
        np.random.seed(seed=seed)
    
    move = np.random.normal(loc=mean, scale=deviation, size=n_steps)
    position = np.insert(move, 0, start_position)
    position = np.cumsum(position)
    
    return position



def add_noise(position, mean=0, deviation=10, seed=None):
    
    if seed is not None:
        np.random.seed(seed=seed)
    
    n_observation = len(position)
    noise = np.random.normal(loc=mean, scale=deviation, size=n_observation)
    observation = position + noise
    
    return observation

Générons en fait une marche aléatoire bruyante. La position de départ est 0. De plus, supposons que la dispersion de marche aléatoire réelle ($ w_t $ dispersion) soit 1 et la dispersion du bruit ($ v_t $ dispersion) soit 10. La moyenne est de 0 pour les deux. Réglez le pas de temps sur 1 ~ 100.

true_position = random_walker(start_position=0, mean=0, deviation=1, n_steps=99, seed=0)
observed_position = add_noise(true_position, mean=0, deviation=10, seed=0)

Faisons un graphique.

plt.plot(true_position, 'r--', label='True Positions')
plt.plot(observed_position, 'y', label='Observed Ppositions')
plt.title('Random Walk')
plt.xlabel('time step')
plt.ylabel('position')
plt.legend(loc='best')

C'est une merveilleuse promenade aléatoire. True Positions est la vraie valeur de la marche aléatoire $ x_t $, et Observed Positions est l'observation bruyante $ y_t $.

La valeur observée est sale.

Filtre de Kalman

Écrivons la formule de filtre de Kalman pour ce modèle de marche aléatoire. Supposons que $ w_t $ suit une distribution gaussienne avec une moyenne de 0 et une variance de $ q $, et que $ v_t $ suit une distribution gaussienne avec une moyenne de 0 et une variance de $ r . La dérivation est omise, mais dans le sens direct du temps\hat{x}_{t|t-1}、\hat{x}_{t|t}$La formule pour trouver

\begin{align}
&\hat{x}_{t/t} = \hat{x}_{t/t-1} + K_t(y_t - \hat{x}_{t/t-1}) \\
&\hat{x}_{t+1/t} = \hat{x}_{t/t} \\
&K_t = \frac{P_{t/t-1}}{P_{t/t-1} + r} \\
&P_{t/t} = \frac{r P_{t/t-1}}{P_{t/t-1} + r} \\
&P_{t+1/t} = P_{t/t} + q \\
\end{align}

La formule pour trouver $ \ hat {x} \ _ {t | N} $ dans l'ordre inverse du temps est

\begin{align}
&\hat{x}_{t/N} = \hat{x}_{t/t} + C_t(\hat{x}_{t+1/N} - \hat{x}_{t+1/t}) \\
&C_t = \frac{P_{t/t}}{P_{t/t} + q} \\
&P_{t/N} = P_{t/t} + C^2_t(P_{t+1/N} - P_{t+1/N})
\end{align}

Ce sera. Implémentons ceci.

class Simple_Kalman:
    
    def __init__(self, observation, start_position, start_deviation, deviation_true, deviation_noise):
       
        self.obs = observation
        self.n_obs = len(observation)
        self.start_pos = start_position
        self.start_dev = start_deviation
        self.dev_q = deviation_true
        self.dev_r = deviation_noise
        
        self._fit()

        
    def _forward(self):
        
        self.x_prev_ = [self.start_pos]
        self.P_prev_ = [self.start_dev]
        self.K_ = [self.P_prev_[0] / (self.P_prev_[0] + self.dev_r)]
        self.P_ = [self.dev_r * self.P_prev_[0] / (self.P_prev_[0] + self.dev_r)]
        self.x_ = [self.x_prev_[0] + self.K_[0] * (self.obs[0] - self.x_prev_[0])]
        
        for t in range(1, self.n_obs):
            self.x_prev_.append(self.x_[t-1])
            self.P_prev_.append(self.P_[t-1] + self.dev_q)
            
            self.K_.append(self.P_prev_[t] / (self.P_prev_[t] + self.dev_r))
            self.x_.append(self.x_prev_[t] + self.K_[t] * (self.obs[t] - self.x_prev_[t]))
            self.P_.append(self.dev_r * self.P_prev_[t] / (self.P_prev_[t] + self.dev_r))

            
    def _backward(self):
        
        self.x_all_ = [self.x_[-1]]
        self.P_all_ = [self.P_[-1]]
        self.C_ = [self.P_[-1] / (self.P_[-1] + self.dev_q)]
        
        for t in range(2, self.n_obs + 1):
            self.C_.append(self.P_[-t] / (self.P_[-t] + self.dev_q))
            self.x_all_.append(self.x_[-t] + self.C_[-1] * (self.x_all_[-1] - self.x_prev_[-t+1]))
            self.P_all_.append(self.P_[-t] + (self.C_[-1]**2) * (self.P_all_[-1] - self.P_prev_[-t+1]))
        
        self.C_.reverse()
        self.x_all_.reverse()
        self.P_all_.reverse()

            
    def _fit(self):
        self._forward()
        self._backward()

Appliquons-le à la marche aléatoire que j'ai mentionnée plus tôt. En pratique, la valeur réelle des marches aléatoires et de la dispersion du bruit est souvent inconnue et doit être estimée. Pour l'instant, utilisons la vraie valeur ici.

kf = Simple_Kalman(observed_position, start_position=0, start_deviation=1, deviation_true=1, deviation_noise=10)

Faisons un graphique.

plt.plot(true_position, 'r--', label='True Positions')
plt.plot(observed_position, 'y', label='Observed Ppositions')
plt.plot(kf.x_, 'blue' ,label='Foward Estimation')
plt.plot(kf.x_all_, 'black', label='Smoothed Estimation')
plt.title('Random Walk')
plt.xlabel('time step')
plt.ylabel('position')
plt.legend(loc='best')

Optimisation avant\hat{x}\_{t|t}, Optimisation lissée\hat{x}\_{t|N}Représente.

C'est devenu beau.

Les références

[Filtre de Kalman non linéaire](https://www.amazon.co.jp/%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%82%AB%E3%83%AB%E3% 83% 9E% E3% 83% B3% E3% 83% 95% E3% 82% A3% E3% 83% AB% E3% 82% BF-% E7% 89% 87% E5% B1% B1-% E5% BE% B9 / dp / 4254201486 /) Librairie Asakura, Toru Katayama

Les formules sont basées sur ce livre. C'est une magnifique couverture sans bruit.