カルマンフィルタメモ

import numpy as np

class KalmanFilter:
    def __init__(self, A, B, H, Q, R, x0, P0):
        self.A = A  # 状態遷移行列
        self.B = B  # 制御行列
        self.H = H  # 観測行列
        self.Q = Q  # システム雑音の共分散
        self.R = R  # 観測雑音の共分散
        self.x = x0  # 初期状態推定値
        self.P = P0  # 初期状態推定の共分散

    def predict(self, u):
        # 状態予測
        self.x = self.A @ self.x + self.B @ u
        # 共分散予測
        self.P = self.A @ self.P @ self.A.T + self.Q

    def update(self, z):
        # カルマンゲインの計算
        K = self.P @ self.H.T @ np.linalg.inv(self.H @ self.P @ self.H.T + self.R)
        # 状態推定値の更新
        self.x = self.x + K @ (z - self.H @ self.x)
        # 共分散の更新
        self.P = self.P - K @ self.H @ self.P

    def get_state(self):
        return self.x

# パラメータ設定
A = np.array([[1, 1], [0, 1]])  # 状態遷移行列
B = np.array([[0.5], [1]])      # 制御行列
H = np.array([[1, 0]])          # 観測行列
Q = np.array([[0.01, 0], [0, 0.01]])  # システム雑音の共分散
R = np.array([[1]])             # 観測雑音の共分散
x0 = np.array([[0], [1]])       # 初期状態推定値
P0 = np.eye(2)                  # 初期状態推定の共分散

# カルマンフィルターの初期化
kf = KalmanFilter(A, B, H, Q, R, x0, P0)

# データ生成（例としてシンプルなシステム）
num_steps = 50
true_state = np.zeros((2, num_steps))
observations = np.zeros((1, num_steps))

# 初期状態
true_state[:, 0] = x0.flatten()

# システム雑音と観測雑音の標準偏差
system_noise_std = np.sqrt(Q.diagonal())
observation_noise_std = np.sqrt(R.diagonal())

# 状態と観測データの生成
for k in range(1, num_steps):
    true_state[:, k] = A @ true_state[:, k-1] + B @ np.random.randn(1) * system_noise_std
    observations[:, k] = H @ true_state[:, k] + np.random.randn(1) * observation_noise_std

# フィルタリング
filtered_state = np.zeros((2, num_steps))
filtered_state[:, 0] = x0.flatten()

for k in range(1, num_steps):
    kf.predict(np.zeros((1, 1)))  # 制御入力uがゼロの場合
    kf.update(observations[:, k])
    filtered_state[:, k] = kf.get_state().flatten()

# 結果のプロット
import matplotlib.pyplot as plt

plt.figure(figsize=(10, 8))
plt.plot(true_state[0, :], label='True Position')
plt.plot(observations.flatten(), label='Observations', linestyle='dotted')
plt.plot(filtered_state[0, :], label='Filtered Position')
plt.legend()
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.title('Kalman Filter - Position Estimation')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from scipy.ndimage import gaussian_filter
from scipy.stats import multivariate_normal

# 1. Kalman Filter
true_value = 10
observations = true_value + np.random.normal(0, 1, 100)

x = 0.0  # Initial state estimate
P = 1.0  # Initial estimate error covariance
Q = 1e-5  # Process noise covariance
R = 1.0  # Measurement noise covariance

filtered_values = []

for z in observations:
    x = x  # State prediction
    P = P + Q  # Covariance prediction
    K = P / (P + R)  # Kalman gain
    x = x + K * (z - x)  # State update
    P = (1 - K) * P  # Covariance update
    filtered_values.append(x)

plt.figure(figsize=(10, 6))
plt.plot(observations, label='Observations', linestyle='dotted')
plt.plot(filtered_values, label='Kalman Filter Estimates', linestyle='solid')
plt.axhline(true_value, color='red', linestyle='dashed', label='True Value')
plt.legend()
plt.title('Kalman Filter')
plt.show()

# 2. Gaussian Filter
data = np.sin(np.linspace(0, 20, 100)) + np.random.normal(0, 0.5, 100)
smoothed_data = gaussian_filter(data, sigma=2)

plt.figure(figsize=(10, 6))
plt.plot(data, label='Original Data', linestyle='dotted')
plt.plot(smoothed_data, label='Gaussian Filtered Data', linestyle='solid')
plt.legend()
plt.title('Gaussian Filter')
plt.show()

# 3. Multivariate Gaussian Distribution, Mean, Covariance Matrix
mean = [0, 0]
cov = [[1, 0.5], [0.5, 1]]
data = np.random.multivariate_normal(mean, cov, 500)

plt.figure(figsize=(10, 6))
plt.scatter(data[:, 0], data[:, 1], s=10)
plt.xlabel('X1')
plt.ylabel('X2')
plt.title('Multivariate Gaussian Distribution')
plt.grid(True)
plt.axis('equal')
plt.show()

# 4. Bayesian Filter
state_space = np.linspace(-10, 10, 100)
observations = np.linspace(-10, 10, 50)
prior = np.exp(-0.5 * (state_space / 2)**2)
prior /= prior.sum()
observation = 2.0
likelihood = np.exp(-0.5 * ((state_space - observation) / 1.0)**2)
likelihood /= likelihood.sum()
posterior = likelihood * prior
posterior /= posterior.sum()

plt.figure(figsize=(10, 6))
plt.plot(state_space, prior, label='Prior')
plt.plot(state_space, likelihood, label='Likelihood')
plt.plot(state_space, posterior, label='Posterior')
plt.legend()
plt.title('Bayesian Filter')
plt.show()

import numpy as np
import matplotlib.pyplot as plt

# シミュレーションのパラメータ
dt = 0.1  # 時間間隔
num_steps = 100  # ステップ数
true_initial_position = 0  # 真の初期位置
true_initial_velocity = 1  # 真の初期速度

# プロセスノイズと観測ノイズ
process_noise_std = 0.1
measurement_noise_std = 0.5

# 状態遷移行列と観測行列
A = np.array([[1, dt], [0, 1]])  # 状態遷移行列
B = np.array([[0.5 * dt**2], [dt]])  # 制御入力行列
H = np.array([[1, 0]])  # 観測行列

# プロセスノイズ共分散行列と観測ノイズ共分散行列
Q = np.array([[process_noise_std**2, 0], [0, process_noise_std**2]])
R = np.array([[measurement_noise_std**2]])

# 初期状態
x_true = np.array([[true_initial_position], [true_initial_velocity]])
x_est = np.array([[0], [0]])  # 初期推定位置と速度
P = np.eye(2)  # 初期推定誤差共分散行列

# 状態と観測の履歴
true_positions = []
measurements = []
estimated_positions = []

# 制御入力（加速度）
u = np.array([[0]])

# カルマンフィルタのループ
for _ in range(num_steps):
    # 真の状態を更新
    x_true = A @ x_true + B @ u + np.random.multivariate_normal([0, 0], Q).reshape(-1, 1)
    true_positions.append(x_true[0, 0])
    
    # ノイズを加えた観測
    z = H @ x_true + np.random.normal(0, measurement_noise_std)
    measurements.append(z[0, 0])
    
    # カルマンフィルタの予測ステップ
    x_pred = A @ x_est + B @ u
    P_pred = A @ P @ A.T + Q
    
    # カルマンフィルタの更新ステップ
    K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)  # カルマンゲイン
    x_est = x_pred + K @ (z - H @ x_pred)  # 状態推定更新
    P = (np.eye(2) - K @ H) @ P_pred  # 誤差共分散更新
    
    estimated_positions.append(x_est[0, 0])

# プロット
plt.figure(figsize=(10, 6))
plt.plot(true_positions, label='True Position', linestyle='dashed')
plt.plot(measurements, label='Measurements', linestyle='dotted')
plt.plot(estimated_positions, label='Kalman Filter Estimate', linestyle='solid')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.title('Kalman Filter for Robot Control and State Transition')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import lfilter
from statsmodels.tsa.arima_process import ArmaProcess

# ARMAモデルのパラメータ
ar_params = np.array([1, -0.5, 0.25])  # AR係数
ma_params = np.array([1, 0.4, -0.3])   # MA係数

# ARMAプロセスのインスタンスを作成
arma_process = ArmaProcess(ar=ar_params, ma=ma_params)

# データを生成
num_samples = 100
np.random.seed(42)
true_values = arma_process.generate_sample(nsample=num_samples)

# ノイズを加えた観測値
observation_noise_std = 0.5
observations = true_values + np.random.normal(0, observation_noise_std, num_samples)

# カルマンフィルタの設定
n = len(true_values)
A = np.eye(2)
C = np.array([[1, 0]])
Q = np.eye(2) * 0.01
R = np.array([[observation_noise_std ** 2]])

# 初期値
x_est = np.zeros((2, 1))
P = np.eye(2)

# フィルタリング結果を格納するリスト
filtered_values = []

# カルマンフィルタのループ
for z in observations:
    # 予測ステップ
    x_pred = A @ x_est
    P_pred = A @ P @ A.T + Q
    
    # 更新ステップ
    K = P_pred @ C.T @ np.linalg.inv(C @ P_pred @ C.T + R)
    x_est = x_pred + K @ (z - C @ x_pred)
    P = (np.eye(2) - K @ C) @ P_pred
    
    filtered_values.append(x_est[0, 0])

# プロット
plt.figure(figsize=(10, 6))
plt.plot(true_values, label='True Values', linestyle='dashed')
plt.plot(observations, label='Observations', linestyle='dotted')
plt.plot(filtered_values, label='Kalman Filter Estimate', linestyle='solid')
plt.xlabel('Time Step')
plt.ylabel('Value')
plt.legend()
plt.title('Kalman Filter with ARMA Model for Economic Data')
plt.show()

import numpy as np
import matplotlib.pyplot as plt

# システムパラメータ
dt = 1.0  # 時間ステップ
A = np.array([[1, dt], [0, 1]])  # 状態遷移行列
H = np.array([[1, 0]])  # 観測行列
Q = np.array([[1, 0], [0, 3]])  # プロセスノイズ共分散行列
R = np.array([[10]])  # 観測ノイズ共分散行列

# 初期状態
x = np.array([[0], [1]])  # 初期状態 [位置, 速度]
P = np.eye(2)  # 初期推定誤差共分散行列

# 真の状態と観測値
true_positions = []
observations = []

# カルマンフィルタの推定結果
estimated_positions = []

for i in range(50):
    # 真の状態
    x_true = A @ x + np.random.multivariate_normal([0, 0], Q).reshape(-1, 1)
    true_positions.append(x_true[0, 0])
    
    # 観測値
    z = H @ x_true + np.random.normal(0, R)
    observations.append(z[0, 0])
    
    # 予測ステップ
    x_pred = A @ x
    P_pred = A @ P @ A.T + Q
    
    # カルマンゲインの計算
    K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
    
    # 更新ステップ
    x = x_pred + K @ (z - H @ x_pred)
    P = (np.eye(2) - K @ H) @ P_pred
    
    # 推定結果の保存
    estimated_positions.append(x[0, 0])

# プロット
plt.figure(figsize=(10, 6))
plt.plot(true_positions, label='True Position', linestyle='dashed')
plt.plot(observations, label='Observations', linestyle='dotted')
plt.plot(estimated_positions, label='Kalman Filter Estimate', linestyle='solid')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.title('1D Kalman Filter')
plt.show()

import numpy as np
import matplotlib.pyplot as plt

# シミュレーションパラメータ
num_samples = 100
observation_noise_std = 0.5

# 移動平均モデル (MA(2))
ma_params = np.array([0.6, -0.3])
np.random.seed(42)
white_noise = np.random.normal(0, 1, num_samples + len(ma_params))
true_values = np.convolve(white_noise, ma_params, mode='valid')

# ノイズを加えた観測値
observations = true_values + np.random.normal(0, observation_noise_std, len(true_values))

# ベイズ推定のパラメータ
state_space = np.linspace(-3, 3, 100)
prior = np.exp(-0.5 * (state_space / 1.0)**2)
prior /= prior.sum()

# 初期設定
x_est = 0  # 初期推定値
P = 1.0  # 初期推定誤差共分散
Q = 0.01  # プロセスノイズ共分散
R = observation_noise_std ** 2  # 観測ノイズ共分散

# カルマンフィルタの結果を格納するリスト
filtered_values = []
posteriors = []

# カルマンフィルタのループ
for z in observations:
    # ベイズ推定：尤度計算
    likelihood = np.exp(-0.5 * ((state_space - z) / observation_noise_std)**2)
    likelihood /= likelihood.sum()
    
    # 事後分布の計算
    posterior = likelihood * prior
    posterior /= posterior.sum()
    
    # カルマンフィルタの予測ステップ
    x_pred = x_est
    P_pred = P + Q
    
    # カルマンゲインの計算
    K = P_pred / (P_pred + R)
    
    # 更新ステップ
    x_est = x_pred + K * (z - x_pred)
    P = (1 - K) * P_pred
    
    # 事前分布を更新
    prior = posterior
    
    # 結果の保存
    filtered_values.append(x_est)
    posteriors.append(posterior)

# プロット
plt.figure(figsize=(14, 6))

# 真の値、観測値、カルマンフィルタの推定値
plt.subplot(1, 2, 1)
plt.plot(true_values, label='True Values', linestyle='dashed')
plt.plot(observations, label='Observations', linestyle='dotted')
plt.plot(filtered_values, label='Kalman Filter Estimate', linestyle='solid')
plt.xlabel('Time Step')
plt.ylabel('Value')
plt.legend()
plt.title('Kalman Filter with MA Model and Bayesian Estimation')

# 事後分布のプロット
plt.subplot(1, 2, 2)
plt.imshow(np.array(posteriors).T, aspect='auto', origin='lower', extent=[0, len(observations), state_space[0], state_space[-1]])
plt.colorbar(label='Probability')
plt.xlabel('Time Step')
plt.ylabel('State Space')
plt.title('Posterior Distribution Over Time')

plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import butter, filtfilt

# シミュレーションパラメータ
num_samples = 100
observation_noise_std = 0.5

# データ生成 (正弦波 + ノイズ)
np.random.seed(42)
time = np.linspace(0, 1, num_samples)
true_signal = np.sin(2 * np.pi * 5 * time)  # 5Hzの正弦波
observations = true_signal + np.random.normal(0, observation_noise_std, num_samples)

# 低パスフィルタの設計
def low_pass_filter(data, cutoff, fs, order=5):
    nyquist = 0.5 * fs
    normal_cutoff = cutoff / nyquist
    b, a = butter(order, normal_cutoff, btype='low', analog=False)
    filtered_data = filtfilt(b, a, data)
    return filtered_data

# 低パスフィルタでノイズ除去
fs = 1 / (time[1] - time[0])  # サンプリング周波数
cutoff = 10  # カットオフ周波数
filtered_signal = low_pass_filter(observations, cutoff, fs)

# カルマンフィルタの設定
A = np.eye(2)
H = np.array([[1, 0]])
Q = np.eye(2) * 0.01
R = np.array([[observation_noise_std ** 2]])

# 初期設定
x_est = np.zeros((2, 1))
P = np.eye(2)

# カルマンフィルタの結果を格納するリスト
filtered_values = []

# カルマンフィルタのループ
for z in filtered_signal:
    # 予測ステップ
    x_pred = A @ x_est
    P_pred = A @ P @ A.T + Q
    
    # 更新ステップ
    K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
    x_est = x_pred + K @ (z - H @ x_pred)
    P = (np.eye(2) - K @ H) @ P_pred
    
    # 結果の保存
    filtered_values.append(x_est[0, 0])

# プロット
plt.figure(figsize=(14, 6))

# 真の信号、観測値、低パスフィルタによる信号、カルマンフィルタの推定値
plt.subplot(1, 1, 1)
plt.plot(time, true_signal, label='True Signal', linestyle='dashed')
plt.plot(time, observations, label='Noisy Observations', linestyle='dotted')
plt.plot(time, filtered_signal, label='Low-pass Filtered Signal', linestyle='dashdot')
plt.plot(time, filtered_values, label='Kalman Filter Estimate', linestyle='solid')
plt.xlabel('Time')
plt.ylabel('Value')
plt.legend()
plt.title('Low-pass Filtering and Kalman Filtering')
plt.show()

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from statsmodels.tsa.arima_process import ArmaProcess
from statsmodels.tsa.arima.model import ARIMA

# パラメータ設定
np.random.seed(0)

# ARモデル
ar_params = np.array([0.75, -0.25])
ar = np.r_[1, -ar_params]  # AR部分
ma = np.array([1])         # MA部分
AR_object = ArmaProcess(ar, ma)
AR_sample = AR_object.generate_sample(nsample=1000)

# MAモデル
ma_params = np.array([0.65, 0.35])
ar = np.array([1])
ma = np.r_[1, ma_params]  # MA部分
MA_object = ArmaProcess(ar, ma)
MA_sample = MA_object.generate_sample(nsample=1000)

# ARMAモデル
ar_params = np.array([0.75, -0.25])
ma_params = np.array([0.65, 0.35])
ar = np.r_[1, -ar_params]  # AR部分
ma = np.r_[1, ma_params]   # MA部分
ARMA_object = ArmaProcess(ar, ma)
ARMA_sample = ARMA_object.generate_sample(nsample=1000)

# ARIMAモデル
d = 1  # 差分の次数
arima_model = ARIMA(ARMA_sample, order=(2, d, 2))
ARIMA_result = arima_model.fit()
ARIMA_sample = ARIMA_result.predict(start=d, end=1000)

# プロット
plt.figure(figsize=(14, 10))

plt.subplot(4, 1, 1)
plt.plot(AR_sample, color='blue')
plt.title('AR Model')

plt.subplot(4, 1, 2)
plt.plot(MA_sample, color='green')
plt.title('MA Model')

plt.subplot(4, 1, 3)
plt.plot(ARMA_sample, color='red')
plt.title('ARMA Model')

plt.subplot(4, 1, 4)
plt.plot(ARIMA_sample, color='purple')
plt.title('ARIMA Model')

plt.tight_layout()
plt.show()

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from pykalman import KalmanFilter

# Generate sample time series data with a seasonal component
np.random.seed(0)
time = np.arange(100)
seasonal_component = 10 + np.sin(2 * np.pi * time / 10)
trend_component = 0.1 * time
noise = np.random.normal(0, 1, size=100)
data = seasonal_component + trend_component + noise

# Apply Kalman Filter to the data
initial_state_mean = [data[0], 0]
transition_matrix = [[1, 1], [0, 1]]
observation_matrix = [[1, 0]]

kf = KalmanFilter(
    initial_state_mean=initial_state_mean,
    transition_matrices=transition_matrix,
    observation_matrices=observation_matrix
)

state_means, _ = kf.filter(data)
trend_estimate = state_means[:, 0]

# Plot the results
plt.figure(figsize=(10, 6))
plt.plot(time, data, label='Observed Data', linestyle='--', marker='o')
plt.plot(time, trend_estimate, label='Estimated Trend', color='red')
plt.plot(time, data - trend_estimate, label='Seasonal Component', color='green')
plt.legend()
plt.title('Kalman Filter Seasonal Adjustment')
plt.xlabel('Time')
plt.ylabel('Value')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
from sklearn.linear_model import LinearRegression
from sklearn.svm import SVR

# ロトカ・ヴォルテラ方程式
def lotka_volterra(X, t, alpha, beta, delta, gamma):
    prey, predator = X
    dprey_dt = alpha * prey - beta * prey * predator
    dpredator_dt = delta * prey * predator - gamma * predator
    return [dprey_dt, dpredator_dt]

# パラメータ
alpha = 1.0   # 被食者の出生率
beta = 0.1    # 捕食による被食者の死亡率
delta = 0.075 # 捕食された被食者1匹当たりの捕食者の出生率
gamma = 1.5   # 捕食者の死亡率

# 初期の個体数
X0 = [10, 5]

# 時間の範囲
t = np.linspace(0, 50, 500)

# 微分方程式を解く
X = odeint(lotka_volterra, X0, t, args=(alpha, beta, delta, gamma))

# ノイズを追加した観測値
noisy_X = X + np.random.normal(0, 2, X.shape)

# データを学習用とテスト用に分割
split_ratio = 0.8
split_index = int(len(t) * split_ratio)
t_train, t_test = t[:split_index], t[split_index:]
noisy_X_train, noisy_X_test = noisy_X[:split_index], noisy_X[split_index:]

# 線形回帰モデル
linear_model = LinearRegression()
linear_model.fit(t_train.reshape(-1, 1), noisy_X_train)
linear_pred = linear_model.predict(t_test.reshape(-1, 1))

# SVMモデル
svm_model = SVR(kernel='rbf', C=100, gamma=0.1, epsilon=0.1)
svm_model.fit(t_train.reshape(-1, 1), noisy_X_train[:, 0])  # Prey population
svm_pred_prey = svm_model.predict(t_test.reshape(-1, 1))

svm_model.fit(t_train.reshape(-1, 1), noisy_X_train[:, 1])  # Predator population
svm_pred_predator = svm_model.predict(t_test.reshape(-1, 1))

# 結果をプロット
fig, axs = plt.subplots(3, 1, figsize=(12, 18), sharex=True)

# 真の個体数
axs[0].plot(t, X[:, 0], 'g-', label='True Prey Population')
axs[0].plot(t, X[:, 1], 'b-', label='True Predator Population')
axs[0].set_ylabel('Population')
axs[0].legend()
axs[0].set_title('True Population')

# ノイズのある観測値
axs[1].plot(t, noisy_X[:, 0], color='green', alpha=0.5, label='Noisy Prey Observations')
axs[1].plot(t, noisy_X[:, 1], color='blue', alpha=0.5, label='Noisy Predator Observations')
axs[1].set_ylabel('Population')
axs[1].legend()
axs[1].set_title('Noisy Observations')

# 推定結果（線形回帰とSVM）
axs[2].plot(t_test, linear_pred[:, 0], 'r--', linewidth=2, label='Linear Regression Prey')
axs[2].plot(t_test, linear_pred[:, 1], 'c--', linewidth=2, label='Linear Regression Predator')
axs[2].plot(t_test, svm_pred_prey, 'm--', linewidth=2, label='SVM Prey')
axs[2].plot(t_test, svm_pred_predator, 'y--', linewidth=2, label='SVM Predator')
axs[2].set_xlabel('Time')
axs[2].set_ylabel('Population')
axs[2].legend()
axs[2].set_title('Estimates with Linear Regression and SVM')

plt.show()