カルマンフィルタの基礎 


https://www.amazon.co.jp/%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%81%AE%E5%9F%BA%E7%A4%8E-%E8%B6%B3%E7%AB%8B%E4%BF%AE%E4%B8%80/dp/4501328908/ref=sr_1_4?adgrpid=128465005635&dib=eyJ2IjoiMSJ9.RfOHf5WOD4TcM_pfCahm4Au-mbjRYMDC7uU7M5qaNAj4Cs9o-7Eg7mBVUfT1BNiMOgRCZ_S-2o0wRR5dTUQFAXMpmg5WEtkM4xdH8O2Jc_OgLd0t11O_49Z_rFeA8UEtePZolrGi9AKer9X9f4YevJpb-tk9kCB4kFBq3E1waYNSAvI4_qhtWSWg6xS6Ndqd.QlaX1nOWRAxnDtb9EE6ES3o9ESE8LkAtzfCIQMZ_ejU&dib_tag=se&hvadid=679022579698&hvdev=c&hvlocphy=1009341&hvnetw=g&hvqmt=b&hvrand=7717459193680187031&hvtargid=kwd-1423675636722&hydadcr=27267_14738570&jp-ad-ap=0&keywords=%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%81%AE%E5%9F%BA%E7%A4%8E%E3%81%A8%E5%AE%9F%E8%A3%85&qid=1728730530&sr=8-4


P114
```
import numpy as np
import matplotlib.pyplot as plt

# 定数の定義 (Define constants)
A = 1
B = 1
C = 1
Q = 1  # システム雑音の共分散 (System noise covariance)
R = 2  # 観測雑音の共分散 (Observation noise covariance)
N = 300  # データ数 (Number of data points)

# 状態と観測の初期化 (Initialize state and observation)
x = np.zeros(N)
y = np.zeros(N)

# ノイズの生成 (Generate noise)
v = np.random.randn(N) * np.sqrt(Q)  # システム雑音 (System noise)
w = np.random.randn(N) * np.sqrt(R)  # 観測雑音 (Observation noise)

# 状態空間モデルを使ったデータ生成 (Generate data using state-space model)
x[0] = 0  # 初期状態 (Initial state)
y[0] = C * x[0] + w[0]  # 初期観測 (Initial observation)

for k in range(1, N):
    x[k] = A * x[k-1] + B * v[k-1]  # 状態の更新 (State update)
    y[k] = C * x[k] + w[k]  # 観測の更新 (Observation update)

# カルマンフィルタによる状態推定 (Kalman filter state estimation)
xhat = np.zeros(N)  # 状態推定値 (Estimated state)
P = 0  # 誤差共分散 (Error covariance)
xhat[0] = 0  # 初期推定値 (Initial estimate)

# カルマンフィルタの実行 (Run Kalman filter)
for k in range(1, N):
    # 予測ステップ (Prediction step)
    xhatm = A * xhat[k-1]  # 時間更新 (Time update)
    Pm = A * P * A + Q  # 誤差共分散の更新 (Covariance update)
    
    # カルマンゲインの計算 (Calculate Kalman gain)
    G = Pm * C / (C * Pm * C + R)
    
    # 観測更新 (Observation update)
    xhat[k] = xhatm + G * (y[k] - C * xhatm)  # 状態更新 (State update)
    P = (1 - G * C) * Pm  # 誤差共分散の更新 (Covariance update)

# 結果のプロット (Plot results)
plt.figure(figsize=(10,6))
plt.plot(range(N), y, 'k:', label='Measured')
plt.plot(range(N), x, '--', label='True State')
plt.plot(range(N), xhat, 'b', label='Estimated State')
plt.xlabel('No. of samples')
plt.ylabel('Value')
plt.legend()
plt.title('Kalman Filter State Estimation')
plt.grid()
plt.show()
```
P120
```
import numpy as np
import matplotlib.pyplot as plt

# システムの定義 (System definition)
A = np.array([[0, -0.7], [1, -1.5]])  # 状態遷移行列 (State transition matrix)
b = np.array([[0.5], [1]])  # 制御行列 (Control matrix)
c = np.array([0, 1])  # 観測行列 (Observation matrix)

# データ数と雑音の設定 (Data size and noise settings)
N = 100  # データ数 (Number of data points)
Q = 1  # システム雑音の共分散 (System noise covariance)
R = 0.1  # 観測雑音の共分散 (Observation noise covariance)

# 観測データの生成 (Generate observation data)
v = np.random.randn(N, 1) * np.sqrt(Q)  # システム雑音 (System noise)
w = np.random.randn(N, 1) * np.sqrt(R)  # 観測雑音 (Observation noise)

# 状態と観測データの初期化 (Initialize state and observation data)
x = np.zeros((N, 2))  # 状態 (State)
y = np.zeros(N)  # 観測 (Observation)

# 初期状態 (Initial state)
y[0] = np.dot(c, x[0, :]) + w[0]

# 状態空間モデルによるデータ生成 (Generate data using state-space model)
for k in range(1, N):
    x[k, :] = np.dot(A, x[k-1, :]) + b.flatten() * v[k-1]  # 状態の更新 (State update)
    y[k] = np.dot(c, x[k, :]) + w[k]  # 観測の更新 (Observation update)

# カルマンフィルタによる推定 (Kalman filter state estimation)
xhat = np.zeros((N, 2))  # 状態推定値 (Estimated state)
P = np.eye(2)  # 誤差共分散行列 (Error covariance matrix)

# 初期推定値 (Initial estimate)
gamma = 1
P = gamma * np.eye(2)  # 誤差共分散の初期値 (Initial error covariance)
xhat[0, :] = [0, 0]  # 初期推定値 (Initial state estimate)

# カルマンフィルタの更新 (Kalman filter update)
for k in range(1, N):
    # 予測ステップ (Prediction step)
    xhatm = np.dot(A, xhat[k-1, :])  # 時間更新 (Time update)
    Pm = np.dot(A, np.dot(P, A.T)) + Q  # 誤差共分散の更新 (Covariance update)
    
    # カルマンゲインの計算 (Calculate Kalman gain)
    G = np.dot(Pm, c.T) / (np.dot(c, np.dot(Pm, c.T)) + R)
    
    # 観測更新 (Observation update)
    xhat[k, :] = xhatm + G * (y[k] - np.dot(c, xhatm))  # 状態更新 (State update)
    P = Pm - np.outer(G, np.dot(c, Pm))  # 誤差共分散の更新 (Covariance update)

# 結果のプロット (Plot results)
plt.figure(figsize=(10,8))

# x1 の推定結果 (Estimation of x1)
plt.subplot(2, 1, 1)
plt.plot(range(N), x[:, 0], 'r--', label='True x1')
plt.plot(range(N), xhat[:, 0], 'b', label='Estimated x1')
plt.xlabel('Samples')
plt.ylabel('x1')
plt.legend()
plt.grid()

# x2 の推定結果と観測値 (Estimation of x2 and measurements)
plt.subplot(2, 1, 2)
plt.plot(range(N), y, 'k:', label='Measured y')
plt.plot(range(N), x[:, 1], 'r--', label='True x2')
plt.plot(range(N), xhat[:, 1], 'b', label='Estimated x2')
plt.xlabel('Samples')
plt.ylabel('x2')
plt.legend()
plt.grid()

plt.tight_layout()
plt.show()
```
P126
```
import numpy as np
import matplotlib.pyplot as plt

# PRBS信号の生成 (Generate PRBS signal)
def prbs(N):
    return np.sign(np.random.choice([-1, 1], size=N))

# 制御入力(周期127のM系列)の生成 (Generate control input with PRBS)
u = prbs(127)

# システムの定義 (System definition)
A = np.array([[0, -0.7], [1, -1.5]])  # 状態遷移行列 (State transition matrix)
b = np.array([0.5, 1])  # 制御行列 (Control matrix)
c = np.array([0, 1])  # 観測行列 (Observation matrix)
h = np.array([0.5, 1])  # その他の制御行列 (Additional control matrix)

# データ数と雑音の設定 (Data size and noise settings)
N = len(u)  # データ数 (Number of data points)
Q = 1  # システム雑音の共分散 (System noise covariance)
R = 0.1  # 観測雑音の共分散 (Observation noise covariance)

# 観測データの生成 (Generate observation data)
v = np.random.randn(N) * np.sqrt(Q)  # システム雑音 (System noise)
w = np.random.randn(N) * np.sqrt(R)  # 観測雑音 (Observation noise)

# 状態と観測データの初期化 (Initialize state and observation data)
x = np.zeros((N, 2))  # 状態 (State)
y = np.zeros(N)  # 観測 (Observation)

# 初期状態 (Initial state)
y[0] = np.dot(c, x[0, :]) + w[0]

# 状態空間モデルによるデータ生成 (Generate data using state-space model)
for k in range(1, N):
    x[k, :] = np.dot(A, x[k-1, :]) + h * u[k-1] + b * v[k-1]  # 状態の更新 (State update)
    y[k] = np.dot(c, x[k, :]) + w[k]  # 観測の更新 (Observation update)

# カルマンフィルタによる推定 (Kalman filter state estimation)
xhat = np.zeros((N, 2))  # 状態推定値 (Estimated state)
P = np.eye(2)  # 誤差共分散行列 (Error covariance matrix)

# 初期推定値 (Initial estimate)
gamma = 1
P = gamma * np.eye(2)  # 誤差共分散の初期値 (Initial error covariance)
xhat[0, :] = [0, 0]  # 初期推定値 (Initial state estimate)

# カルマンフィルタの更新 (Kalman filter update)
for k in range(1, N):
    # 予測ステップ (Prediction step)
    xhatm = np.dot(A, xhat[k-1, :]) + h * u[k-1]  # 時間更新 (Time update)
    Pm = np.dot(A, np.dot(P, A.T)) + Q  # 誤差共分散の更新 (Covariance update)
    
    # カルマンゲインの計算 (Calculate Kalman gain)
    G = np.dot(Pm, c.T) / (np.dot(c, np.dot(Pm, c.T)) + R)
    
    # 観測更新 (Observation update)
    xhat[k, :] = xhatm + G * (y[k] - np.dot(c, xhatm))  # 状態更新 (State update)
    P = Pm - np.outer(G, np.dot(c, Pm))  # 誤差共分散の更新 (Covariance update)

# 結果のプロット (Plot results)
plt.figure(figsize=(10,10))

# 制御入力 u のプロット (Plot control input u)
plt.subplot(3, 1, 1)
plt.step(range(N), u, 'r')
plt.xlabel('Samples (k)')
plt.ylabel('u')
plt.xlim([1, N])
plt.ylim([1.2 * np.min(u), 1.2 * np.max(u)])

# x1 の推定結果 (Estimation of x1)
plt.subplot(3, 1, 2)
plt.plot(range(N), x[:, 0], 'r', label='True x1')
plt.plot(range(N), xhat[:, 0], 'b', label='Estimated x1')
plt.xlabel('Samples (k)')
plt.ylabel('x1')
plt.legend()
plt.xlim([1, N])

# x2 の推定結果と観測値 (Estimation of x2 and measurements)
plt.subplot(3, 1, 3)
plt.plot(range(N), y, 'k:', label='Measured y')
plt.plot(range(N), x[:, 1], 'r--', label='True x2')
plt.plot(range(N), xhat[:, 1], 'b', label='Estimated x2')
plt.xlabel('Samples (k)')
plt.ylabel('x2')
plt.legend()
plt.xlim([1, N])

plt.tight_layout()
plt.show()
```
P136
```
import numpy as np
import scipy.linalg
from control import ss, lqe

# システム行列 (System matrices)
A = np.array([[0.7683, -0.4940, 0.1129],
              [0.6202,  0,      0     ],
              [-0.0817, 1,      0     ]])

B = np.array([[-0.3832],
              [ 0.5919],
              [ 0.5191]])

C = np.array([[0, 1, 0]])  # 観測行列 (Observation matrix)

# システム雑音と観測雑音の分散 (System noise covariance and observation noise covariance)
Q = np.array([[1]])
R = np.array([[1]])

# 離散時間状態空間モデル (State-space model)
Plant = ss(A, B, C, 0, -1)  # -1 means discrete-time system

# 定常カルマンフィルタの計算 (Steady-state Kalman filter calculation)
P, L, K = lqe(A, B, C, Q, R)

# カルマンゲイン (Kalman gain)
print("Kalman gain (g):\n", K)

# 得られたカルマンフィルタの状態空間モデル (Steady-state Kalman filter state-space model)
# This corresponds to the Kalman filter's state-space form
Akf = A - np.dot(K, C)
Bkf = np.hstack([B, K])
Ckf = np.vstack([C, np.zeros_like(C)])
Dkf = np.array([[0, 0]])

# カルマンフィルタの状態空間モデルを出力 (Display the state-space model)
print("State-space model (A):\n", Akf)
print("State-space model (B):\n", Bkf)
print("State-space model (C):\n", Ckf)
print("State-space model (D):\n", Dkf)

```


P144
```
import numpy as np
import matplotlib.pyplot as plt

# パラメータ設定 (Parameter settings)
n = 100  # データ数 (Number of data points)
lam = 0.99  # 忘却係数 (Forgetting factor)
delta = 1000  # 初期誤差共分散行列のスケーリング (Initial P scaling)

# システムの真のパラメータ (True system parameters)
true_theta = np.array([1.5, -0.7, 1.0, 0.5])  # 真のパラメータ (a1, a2, b1, b2)

# 入力信号 (Input signal)
u = np.random.randn(n)

# 雑音 (Noise)
w = 0.1 * np.random.randn(n)

# 出力信号の初期化 (Initialize output signal)
y = np.zeros(n)

# 差分方程式を使って出力データを生成 (Generate output data using difference equation)
for k in range(2, n):
    y[k] = 1.5 * y[k-1] - 0.7 * y[k-2] + u[k-1] + 0.5 * u[k-2] + w[k]

# RLSによるパラメータ推定の初期化 (Initialize RLS parameters)
theta_hat = np.zeros((4, n))  # 推定されるパラメータ (Estimated parameters)
P = delta * np.eye(4)  # 誤差共分散行列 (Error covariance matrix)

# 推定のためのデータの準備 (Prepare data for estimation)
for k in range(2, n):
    phi_k = np.array([y[k-1], y[k-2], u[k-1], u[k-2]])  # 回帰ベクトル (Regression vector)

    # カルマンゲインの計算 (Compute Kalman gain)
    K = P @ phi_k / (lam + phi_k.T @ P @ phi_k)
    
    # パラメータの更新 (Update parameters)
    theta_hat[:, k] = theta_hat[:, k-1] + K * (y[k] - phi_k.T @ theta_hat[:, k-1])
    
    # 誤差共分散行列の更新 (Update error covariance matrix)
    P = (P - np.outer(K, phi_k.T @ P)) / lam

# 結果のプロット (Plot results)
plt.figure(figsize=(10, 6))

# 推定されたパラメータのプロット (Plot estimated parameters)
for i in range(4):
    plt.plot(theta_hat[i, :], label=f'Estimated theta_{i+1}')
    
# 真のパラメータのプロット (Plot true parameters)
for i, true_param in enumerate(true_theta):
    plt.axhline(y=true_param, color='k', linestyle='--', label=f'True theta_{i+1}')

plt.xlabel('Time (k)')
plt.ylabel('Parameter value')
plt.legend()
plt.grid(True)
plt.title('RLS Parameter Estimation')
plt.show()

# 推定された最終的なパラメータを表示 (Display final estimated parameters)
print("Final estimated parameters:", theta_hat[:, -1])
```
P146
```
import numpy as np
import matplotlib.pyplot as plt

# パラメータ設定 (Parameter settings)
n = 50  # データ数 (Number of data points)
lam = 1.0  # 忘却係数 (Forgetting factor)
delta = 1.0  # 初期誤差共分散行列のスケーリング (Initial P scaling)

# システムの真のパラメータ (True system parameters)
true_theta = np.array([1.0, 0.5, -1.5, 0.7])  # 真のパラメータ (b1, b2, a1, a2)

# 入力信号 (Input signal)
u = np.sign(np.random.randn(n))  # 符号付きランダム信号

# 雑音 (Noise)
w = 0.2 * np.random.randn(n)

# 出力信号の初期化 (Initialize output signal)
y = np.zeros(n)

# 差分方程式を使って出力データを生成 (Generate output data using difference equation)
for k in range(2, n):
    y[k] = 1.5 * y[k-1] - 0.7 * y[k-2] + u[k-1] + 0.5 * u[k-2] + w[k]

# RLSによるパラメータ推定の初期化 (Initialize RLS parameters)
theta_hat = np.zeros((4, n))  # 推定されるパラメータ (Estimated parameters)
P = delta * np.eye(4)  # 誤差共分散行列 (Error covariance matrix)

# 推定のためのデータの準備 (Prepare data for estimation)
for k in range(2, n):
    phi_k = np.array([-y[k-1], -y[k-2], u[k-1], u[k-2]])  # 回帰ベクトル (Regression vector)

    # カルマンゲインの計算 (Compute Kalman gain)
    K = P @ phi_k / (lam + phi_k.T @ P @ phi_k)
    
    # パラメータの更新 (Update parameters)
    theta_hat[:, k] = theta_hat[:, k-1] + K * (y[k] - phi_k.T @ theta_hat[:, k-1])
    
    # 誤差共分散行列の更新 (Update error covariance matrix)
    P = (P - np.outer(K, phi_k.T @ P)) / lam

# 結果のプロット (Plot results)
plt.figure(figsize=(10, 6))

# 推定されたパラメータのプロット (Plot estimated parameters)
for i in range(4):
    plt.plot(theta_hat[i, :], label=f'Estimated theta_{i+1}')
    
# 真のパラメータのプロット (Plot true parameters)
for i, true_param in enumerate(true_theta):
    plt.axhline(y=true_param, color='k', linestyle='--', label=f'True theta_{i+1}')

plt.xlabel('Time (k)')
plt.ylabel('Parameter value')
plt.legend()
plt.grid(True)
plt.title('RLS Parameter Estimation')
plt.show()

# 推定された最終的なパラメータを表示 (Display final estimated parameters)
print("Final estimated parameters:", theta_hat[:, -1])
```
P161
```
import numpy as np
import matplotlib.pyplot as plt

# 非線形システムの関数 (Nonlinear system functions)
def f(x):
    return x + 3 * np.cos(x / 10)

def h(x):
    return x**2 + 3 / 10 * np.sin(x / 10)

def F_jacobian(x):
    return 1 - 3 / 10 * np.sin(x / 10)  # fのヤコビアン (Jacobian of f)

def H_jacobian(x):
    return 2 * x + 3 / 10 * np.cos(x / 10)  # hのヤコビアン (Jacobian of h)

# パラメータの設定 (Parameter settings)
N = 50  # データ数 (Number of data points)
Q = 1  # システム雑音の分散 (System noise variance)
R = 100  # 観測雑音の分散 (Measurement noise variance)

# システム雑音と観測雑音の生成 (Generate system and measurement noise)
v = np.random.randn(N) * np.sqrt(Q)  # システム雑音 (System noise)
w = np.random.randn(N) * np.sqrt(R)  # 観測雑音 (Measurement noise)

# 初期化 (Initialize)
x = np.zeros(N)
y = np.zeros(N)
x[0] = 10
y[0] = h(x[0]) + w[0]

# システムのシミュレーション (System simulation)
for k in range(1, N):
    x[k] = f(x[k-1]) + v[k-1]
    y[k] = h(x[k]) + w[k]

# 拡張カルマンフィルタ (Extended Kalman Filter - EKF)
xhat = np.zeros(N)  # 推定値 (Estimated states)
P = np.zeros(N)  # 誤差共分散行列 (Error covariance)
xhat[0] = 10
P[0] = 1  # 初期推定誤差共分散 (Initial estimate error covariance)

for k in range(1, N):
    # 予測ステップ (Prediction step)
    xhatm = f(xhat[k-1])  # 事前推定 (Predicted state)
    Pm = F_jacobian(xhat[k-1]) * P[k-1] * F_jacobian(xhat[k-1]).T + Q  # 事前誤差共分散 (Predicted error covariance)
    
    # 観測更新ステップ (Measurement update step)
    G = Pm * H_jacobian(xhatm).T / (H_jacobian(xhatm) * Pm * H_jacobian(xhatm).T + R)  # カルマンゲイン (Kalman gain)
    xhat[k] = xhatm + G * (y[k] - h(xhatm))  # 事後推定 (Updated state)
    P[k] = (1 - G * H_jacobian(xhatm)) * Pm  # 事後誤差共分散 (Updated error covariance)

# 結果の表示 (Plot results)
plt.figure()
plt.plot(range(N), x, label='True state')
plt.plot(range(N), xhat, label='Estimated state (EKF)')
plt.xlabel('Time step')
plt.ylabel('State')
plt.legend()
plt.title('Extended Kalman Filter (EKF) - State Estimation')
plt.grid(True)
plt.show()

# 推定された最終的な状態を表示 (Display final estimated state)
print("Final estimated state:", xhat[-1])
```
P171
```
import numpy as np
import matplotlib.pyplot as plt

# 非線形変換関数 (Nonlinear transformation functions)
def f(x):
    return np.array([x[0] * np.cos(x[1]), x[0] * np.sin(x[1])])

# ヤコビアン行列 (Jacobian matrix of f)
def F_jacobian(x):
    return np.array([[np.cos(x[1]), -x[0] * np.sin(x[1])],
                     [np.sin(x[1]), x[0] * np.cos(x[1])]])

# UKFのシグマポイント計算用の関数 (Sigma point calculation for UKF)
def sigma_points(mu, sigma, kappa):
    n = len(mu)
    sigma_sqrt = np.linalg.cholesky((n + kappa) * sigma)
    points = np.zeros((2 * n + 1, n))
    points[0] = mu
    for i in range(n):
        points[i + 1] = mu + sigma_sqrt[i]
        points[i + 1 + n] = mu - sigma_sqrt[i]
    return points

# UKFアルゴリズム (UKF algorithm)
def ukf(mu, sigma, y, Q, R, kappa=1):
    n = len(mu)
    
    # シグマポイントの生成 (Generate sigma points)
    sigma_pts = sigma_points(mu, sigma, kappa)
    
    # 各シグマポイントの変換 (Transform each sigma point)
    Y = np.array([f(pt) for pt in sigma_pts])
    
    # 重みの計算 (Calculate weights)
    w0 = kappa / (n + kappa)
    wi = 1 / (2 * (n + kappa))
    
    # 予測される平均 (Predicted mean)
    y_pred = w0 * Y[0]
    for i in range(1, 2 * n + 1):
        y_pred += wi * Y[i]
    
    # 共分散行列の計算 (Calculate covariance matrices)
    Pyy = w0 * np.outer(Y[0] - y_pred, Y[0] - y_pred)
    for i in range(1, 2 * n + 1):
        Pyy += wi * np.outer(Y[i] - y_pred, Y[i] - y_pred)
    
    Pyy += R
    
    # クロス共分散行列の計算 (Cross-covariance matrix)
    Pxy = w0 * np.outer(sigma_pts[0] - mu, Y[0] - y_pred)
    for i in range(1, 2 * n + 1):
        Pxy += wi * np.outer(sigma_pts[i] - mu, Y[i] - y_pred)
    
    # カルマンゲインの計算 (Calculate Kalman gain)
    K = np.dot(Pxy, np.linalg.inv(Pyy))
    
    # 状態と共分散行列の更新 (Update state and covariance)
    mu = mu + np.dot(K, (y - y_pred))
    sigma = sigma - np.dot(K, np.dot(Pyy, K.T))
    
    return mu, sigma

# 初期パラメータの設定 (Initial parameter setup)
N = 500
mu = np.array([10, np.pi / 2])
sigma = np.diag([50, 1])
Q = np.diag([1, 0.025])
R = 1

# サンプルデータの生成 (Generate sample data)
np.random.seed(0)
x = np.random.multivariate_normal(mu, sigma, N)

# 非線形関数fを使用して出力データを生成 (Generate output data using function f)
y = np.array([f(x_i) for x_i in x])

# UKFの実行 (Run UKF)
mu_est = np.copy(mu)
sigma_est = np.copy(sigma)
mu_estimates = []
sigma_estimates = []

for k in range(N):
    mu_est, sigma_est = ukf(mu_est, sigma_est, y[k], Q, R)
    mu_estimates.append(mu_est)
    sigma_estimates.append(sigma_est)

mu_estimates = np.array(mu_estimates)

# 結果のプロット (Plot results)
plt.figure()
plt.plot(x[:, 0], x[:, 1], 'r.', label='True states')
plt.plot(mu_estimates[:, 0], mu_estimates[:, 1], 'b-', label='UKF estimated states')
plt.xlabel('x1')
plt.ylabel('x2')
plt.legend()
plt.title('UKF State Estimation')
plt.grid(True)
plt.show()

```
P178
```
import numpy as np
import matplotlib.pyplot as plt

# 定数 (Physical parameters)
rho = 1.23  # 空気密度 (air density)
g = 9.81  # 重力加速度 (gravity)
eta = 6e3  # 抗力定数 (drag constant)
M = 3e4  # 質量 (mass)
a = 3e4  # 特定の定数 (specific constant)

# シミュレーションパラメータ (Simulation parameters)
T = 0.5  # オイラー法離散化周期 (discretization step)
EndTime = 30  # シミュレーション終了時刻 (simulation end time)
time = np.arange(0, EndTime + T, T)  # 時間ステップ (time steps)
N = len(time)  # データ数 (number of data points)
n = 3  # 状態変数の数 (number of state variables)
R = 4e3  # 観測雑音の分散 (measurement noise variance)
Q = 0  # システム雑音なし (no system noise)

# 初期状態 (Initial state)
x_init = np.array([90000, -6000, 0.003])

# システム方程式 f(x) (System equation f(x))
def f(x):
    return np.array([
        x[0] + T * x[1],
        x[1] + T * (0.5 * rho * np.exp(-x[0] / eta) * x[1]**2 * x[2] - g),
        x[2]
    ])

# 観測方程式 h(x) (Measurement equation h(x))
def h(x):
    return np.sqrt(M**2 + (x[0] - a)**2)

# f のヤコビアン (Jacobian of f for EKF)
def A_jacobian(x):
    return np.array([
        [1, T, 0],
        [-0.5 * T * rho / eta * np.exp(-x[0] / eta) * x[1]**2 * x[2], 
         1 + T * rho * np.exp(-x[0] / eta) * x[1] * x[2], 
         0.5 * T * rho * np.exp(-x[0] / eta) * x[1]**2],
        [0, 0, 1]
    ])

# h のヤコビアン (Jacobian of h for EKF)
def C_jacobian(x):
    return np.array([(x[0] - a) / np.sqrt(M**2 + (x[0] - a)**2), 0, 0])

# 観測データ生成 (Generate measurement data)
w = np.random.randn(N) * np.sqrt(R)  # 観測雑音 (measurement noise)
x = np.zeros((N, n))  # 状態変数の記憶領域 (storage for state variables)
y = np.zeros(N)  # 観測データの記憶領域 (storage for measurements)

x[0] = x_init  # 初期状態 (initial state)
y[0] = h(x[0]) + w[0]  # 初期観測データ (initial measurement)

# 状態の時間更新 (State time update)
for k in range(1, N):
    x[k] = f(x[k-1])  # 次の状態 (next state)
    y[k] = h(x[k]) + w[k]  # 観測データ (measurement data)

# 結果をプロット (Plot results)
plt.figure()
plt.plot(time, y, 'r.', label='Measurements')
plt.plot(time, h(x.T), 'b-', label='True values')
plt.xlabel('Time [s]')
plt.ylabel('Measurement')
plt.legend()
plt.title('State and Measurements over Time')
plt.grid(True)
plt.show()
```
P183
```
import numpy as np
import matplotlib.pyplot as plt

# パラメータ設定 (Parameter setup)
N = 50  # データ数 (Number of data points)
n = 1  # 状態変数の個数 (Number of state variables)
Q = 1  # システム雑音の分散 (System noise variance)
R = 3  # 観測雑音の分散 (Measurement noise variance)
kappa = 2  # UKFのスケーリングパラメータ (Scaling parameter for UKF)

# システムモデル (System model)
def f(x, k):
    return 0.2 * x + 25 * x / (1 + x**2) + 8 * np.cos(1.2 * k)

def h(x):
    return x**2 / 20

# fのヤコビアン (Jacobian of f for EKF)
def A_jacobian(x):
    return 0.2 + 25 * (1 - x**2) / (1 + x**2)**2

# hのヤコビアン (Jacobian of h for EKF)
def C_jacobian(x):
    return x / 10

# 観測データの生成 (Generate measurement data)
v = np.random.randn(N) * np.sqrt(Q)  # システム雑音 (system noise)
w = np.random.randn(N) * np.sqrt(R)  # 観測雑音 (measurement noise)
x = np.zeros(N)  # 真の状態 (True state)
y = np.zeros(N)  # 観測データ (Measurement data)

x[0] = 0  # 初期状態 (Initial state)
y[0] = h(x[0]) + w[0]  # 初期観測データ (Initial measurement)

# 真の状態と観測データの時間更新 (Update true state and measurements over time)
for k in range(1, N):
    x[k] = f(x[k-1], k) + v[k-1]
    y[k] = h(x[k]) + w[k]

# EKFアルゴリズム (EKF Algorithm)
xhat_ekf = np.zeros(N)  # EKFによる推定値 (EKF estimated state)
P_ekf = 1  # 誤差共分散行列の初期値 (Initial error covariance)

for k in range(1, N):
    # 予測ステップ (Prediction step)
    xhat_ekf[k] = f(xhat_ekf[k-1], k)
    P_ekf = A_jacobian(xhat_ekf[k-1]) * P_ekf * A_jacobian(xhat_ekf[k-1]) + Q
    
    # カルマンゲイン (Kalman gain)
    K_ekf = P_ekf * C_jacobian(xhat_ekf[k]) / (C_jacobian(xhat_ekf[k]) * P_ekf * C_jacobian(xhat_ekf[k]) + R)
    
    # 観測更新ステップ (Measurement update step)
    xhat_ekf[k] += K_ekf * (y[k] - h(xhat_ekf[k]))
    P_ekf = (1 - K_ekf * C_jacobian(xhat_ekf[k])) * P_ekf

# UKFアルゴリズム (UKF Algorithm)
xhat_ukf = np.zeros(N)  # UKFによる推定値 (UKF estimated state)
P_ukf = 1  # 誤差共分散行列の初期値 (Initial error covariance)

def sigma_points(x, P, kappa):
    n = len(x)
    sigmas = np.zeros((2 * n + 1, n))
    sigmas[0] = x
    sqrt_P = np.sqrt((n + kappa) * P)
    for i in range(n):
        sigmas[i + 1] = x + sqrt_P[i]
        sigmas[i + n + 1] = x - sqrt_P[i]
    return sigmas

for k in range(1, N):
    # シグマポイントの生成 (Generate sigma points)
    sigmas = sigma_points(np.array([xhat_ukf[k-1]]), P_ukf, kappa)
    
    # シグマポイントの変換 (Transform sigma points)
    sigmas_f = np.array([f(sigma[0], k) for sigma in sigmas])
    
    # 予測ステップ (Prediction step)
    xhat_ukf[k] = np.mean(sigmas_f)
    P_ukf = np.var(sigmas_f) + Q
    
    # 観測変換 (Measurement transformation)
    sigmas_h = np.array([h(sigma_f) for sigma_f in sigmas_f])
    
    # カルマンゲインの計算 (Kalman gain)
    P_yy = np.var(sigmas_h) + R
    P_xy = np.mean((sigmas_f - xhat_ukf[k]) * (sigmas_h - np.mean(sigmas_h)))
    K_ukf = P_xy / P_yy
    
    # 観測更新 (Measurement update)
    xhat_ukf[k] += K_ukf * (y[k] - np.mean(sigmas_h))
    P_ukf -= K_ukf * P_yy * K_ukf

# 結果のプロット (Plot results)
plt.figure()
plt.plot(range(N), x, 'k-', label='True state')
plt.plot(range(N), xhat_ekf, 'b--', label='EKF estimate')
plt.plot(range(N), xhat_ukf, 'r-.', label='UKF estimate')
plt.xlabel('k')
plt.ylabel('x')
plt.legend()
plt.grid(True)
plt.show()

# RMSEの計算 (Calculate RMSE)
rmse_ekf = np.sqrt(np.mean((x - xhat_ekf)**2))
rmse_ukf = np.sqrt(np.mean((x - xhat_ukf)**2))
print(f"RMSE (EKF): {rmse_ekf:.3f}")
print(f"RMSE (UKF): {rmse_ukf:.3f}")
```
P187
```
import numpy as np
import matplotlib.pyplot as plt

# 物理パラメータの設定 (Physical parameter setup)
m = 2.0  # 質量 (mass)
k = 0.7  # バネ定数 (spring constant)
c_true = 1.0  # 真の摩擦係数 (true damping coefficient)
dT = 0.01  # 離散化時間ステップ (time step)
N = 2000  # データ数 (number of data points)
t = np.arange(0, N) * dT  # 時間 (time)
u = 4 * np.sawtooth(t * np.sqrt(2)) + 10 * np.sin(t)  # 入力信号 (input signal)

# 状態遷移方程式 (State transition equation)
def system_dynamics(x, u, c):
    x1, x2 = x
    dxdt = np.array([x2, (-k/m)*x1 - (c/m)*x2 + (1/m)*u])
    return dxdt

# 観測方程式 (Observation equation)
def observation(x):
    return x[0]

# システム雑音と観測雑音の設定 (Set system and measurement noise)
Q = np.diag([1e-5, 1e-5])  # システム雑音の共分散行列 (system noise covariance)
R = 0.1  # 観測雑音の分散 (measurement noise variance)

# 初期状態 (Initial state)
x_true = np.zeros((N, 2))
x_true[0] = [0, 0]  # 初期状態 (initial condition)

# シミュレーションによる真の状態と観測データの生成 (Generate true state and observations)
y = np.zeros(N)
for n in range(1, N):
    x_true[n] = x_true[n-1] + dT * system_dynamics(x_true[n-1], u[n], c_true)
    y[n] = observation(x_true[n]) + np.random.randn() * np.sqrt(R)

# EKFによるパラメータ推定 (EKF for parameter estimation)
xhat = np.zeros((N, 3))  # 状態推定 (state estimate)
P = np.diag([10, 10, 10])  # 誤差共分散行列 (initial covariance)
xhat[0] = [0, 0, 0.1]  # 初期推定値 (initial estimates)

# EKFの更新 (EKF update step)
for n in range(1, N):
    # 予測ステップ (Prediction step)
    A = np.array([[0, 1, 0],
                  [-k/m, -xhat[n-1, 2]/m, -xhat[n-1, 1]/m],
                  [0, 0, 0]])
    xhat_pred = xhat[n-1] + dT * np.array([xhat[n-1, 1], (-k/m)*xhat[n-1, 0] - (xhat[n-1, 2]/m)*xhat[n-1, 1] + (1/m)*u[n], 0])
    P_pred = A @ P @ A.T + Q

    # 観測更新ステップ (Measurement update step)
    C = np.array([1, 0, 0])
    K = P_pred @ C.T / (C @ P_pred @ C.T + R)
    y_pred = C @ xhat_pred
    xhat[n] = xhat_pred + K * (y[n] - y_pred)
    P = (np.eye(3) - np.outer(K, C)) @ P_pred

# 結果のプロット (Plot results)
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(t, x_true[:, 0], 'r', label='True Position')
plt.plot(t, xhat[:, 0], 'b', label='Estimated Position')
plt.ylabel('Position')
plt.legend()

plt.subplot(3, 1, 2)
plt.plot(t, x_true[:, 1], 'r', label='True Velocity')
plt.plot(t, xhat[:, 1], 'b', label='Estimated Velocity')
plt.ylabel('Velocity')
plt.legend()

plt.subplot(3, 1, 3)
plt.plot(t, np.ones(N) * c_true, 'r', label='True Parameter c')
plt.plot(t, xhat[:, 2], 'b', label='Estimated Parameter c')
plt.xlabel('Time [s]')
plt.ylabel('Parameter c')
plt.legend()

plt.show()
P202

import numpy as np
import matplotlib.pyplot as plt

# データ数 (Number of data points)
N = 200

# 真値の生成 (Generate true values)
z = np.zeros(N)
d = np.random.randn(N) * np.sqrt(5)
z[0] = 10
for k in range(1, N):
    z[k] = z[k-1] + 2 * np.cos(0.05 * k) + d[k-1]

# センサ1とセンサ2の出力生成 (Generate sensor outputs)
a = 0.75
A = np.array([[a, 0, 0], [0, 1, 0], [0, 0, 0]])
B = np.array([[1], [1], [0]])
Q = np.diag([60, 60])
R = 1e-4
C = np.array([[1, 1, -1]])

# センサ誤差の生成 (Generate sensor errors)
v = np.random.randn(N, 2) @ np.sqrt(Q)
x = np.zeros((N, 3))
x[0] = [10, 20, 0]  # 初期状態 (Initial state)

# 時間更新 (Time update)
for k in range(1, N):
    x[k] = A @ x[k-1] + B @ v[k-1]

# センサの出力 (Sensor outputs)
e1 = z + x[:, 0]  # センサ1の測定誤差 (Sensor 1 measurement error)
e2 = x[:, 2]      # センサ2の測定誤差 (Sensor 2 measurement error)

# 相補フィルタを適用した出力 (Apply complementary filter)
y1 = z + e1
y2 = z + e2
y = y1 - y2

# 結果のプロット (Plot results)
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(range(N), z, 'k-', label='True')
plt.plot(range(N), y1, 'r--', label='Sensor1')
plt.legend()
plt.xlabel('k')
plt.ylabel('z')

plt.subplot(3, 1, 2)
plt.plot(range(N), z, 'k-', label='True')
plt.plot(range(N), y2, 'r--', label='Sensor2')
plt.legend()
plt.xlabel('k')
plt.ylabel('z')

plt.subplot(3, 1, 3)
plt.plot(range(N), z, 'k-', label='True')
plt.plot(range(N), y, 'b--', label='Corrected')
plt.legend()
plt.xlabel('k')
plt.ylabel('z')

plt.tight_layout()
plt.show()

```


P29
```

import numpy as np
import matplotlib.pyplot as plt

# Define system matrices (A, b, c) / システムの行列A, b, cを定義
A = np.array([[-0.5, 1.0], [-0.3, 0.0]])  # State transition matrix / 状態遷移行列
b = np.array([[1.0], [1.0]])  # Control matrix / 制御行列
c = np.array([[-2.5, 3.5]])  # Observation matrix / 観測行列

# Initial state / 初期状態
x = np.array([[0], [0]])

# Generate system noise (v) and observation noise (w) / システム雑音vと観測雑音wを生成
v = np.random.normal(0, 1, (1, 100))  # System noise / システム雑音
w = np.random.normal(0, 1, (1, 100))  # Observation noise / 観測雑音

# Initialize lists to store states and outputs / 状態ベクトルと出力yを保存するリストを初期化
states = []
y = []

# Simulate over 100 time steps / 100ステップのシミュレーション
for k in range(100):
    # Update the state equation / 状態方程式を更新
    x = A @ x + b * v[0, k]
    # Calculate the output / 出力yを計算
    y_k = c @ x + w[0, k]
    # Save the state and output / 状態と出力を保存
    states.append(x.flatten())
    y.append(y_k.flatten())

# Convert lists to numpy arrays for easier plotting / リストをNumPy配列に変換
states = np.array(states)
y = np.array(y)

# Plot the states and output / 状態ベクトルと出力yをプロット
plt.figure(figsize=(10, 6))

# Plot state variables / 状態変数をプロット
plt.subplot(2, 1, 1)
plt.plot(states[:, 0], label='State x1')
plt.plot(states[:, 1], label='State x2')
plt.title('State Variables Over Time')
plt.ylabel('State Value')
plt.legend()

# Plot output y / 出力yをプロット
plt.subplot(2, 1, 2)
plt.plot(y, label='Output y')
plt.title('Output y Over Time')
plt.xlabel('Time Step')
plt.ylabel('Output Value')
plt.legend()

plt.tight_layout()
plt.show()
```

P34
```

import numpy as np
import matplotlib.pyplot as plt

# System parameters (a1, a2, b0, b1, b2)
# ARMAモデルの係数 a1, a2, b0, b1, b2 を設定
a1 = 1.2  # Example value / 例の値
a2 = -0.5  # Example value / 例の値
b0 = 1.0  # Example value / 例の値
b1 = 0.5  # Example value / 例の値
b2 = 0.2  # Example value / 例の値

# Define the state-space matrices based on the ARMA model
# ARMAモデルからの状態空間行列を定義
A = np.array([[0, 1], [-a2, -a1]])  # State transition matrix / 状態遷移行列
B = np.array([[0], [1]])  # Input matrix / 入力行列
C = np.array([[b2, b1]])  # Output matrix / 出力行列
D = b0  # Direct term / 直接項

# Initialize the state vector / 状態ベクトルを初期化
x = np.array([[0], [0]])  # Initial state / 初期状態

# Time steps and noise generation / 時間ステップとノイズ生成
n_steps = 100  # Number of time steps / 時間ステップの数
v = np.random.normal(0, 1, (n_steps, 1))  # Input noise (system noise) / 入力雑音（システム雑音）
y = np.zeros(n_steps)  # Output / 出力を初期化

# Simulation loop / シミュレーションループ
for k in range(n_steps):
    # State update equation / 状態更新方程式
    x = A @ x + B * v[k]
    
    # Output equation / 出力方程式
    y[k] = C @ x + D * v[k]

# Plot the results / 結果をプロット
plt.figure(figsize=(10, 6))

# Plot the output y / 出力yのプロット
plt.plot(y, label='Output y')
plt.title('Output y Over Time (ARMA Model)')
plt.xlabel('Time Step')
plt.ylabel('Output')
plt.legend()

plt.tight_layout()
plt.show()
```

P36
```

import numpy as np
import matplotlib.pyplot as plt

# Define ARMA model coefficients (a1, a2, b0, b1, b2)
# ARMAモデルの係数 a1, a2, b0, b1, b2 を定義する
a1 = 0.15  # Example value / 例題の値
a2 = 0.8   # Example value / 例題の値
b0 = 1.0   # Example value / 例題の値
b1 = 1.0   # Example value / 例題の値
b2 = 1.0   # Example value / 例題の値

# Define state-space matrices (A, B, C, D)
# 状態空間行列 A, B, C, D を定義する
A = np.array([[0, 1], [-a2, -a1]])  # State transition matrix / 状態遷移行列
B = np.array([[0], [1]])  # Input matrix / 入力行列
C = np.array([b2, b1])    # Output matrix / 出力行列
D = b0  # Direct term / 直接項

# Initial state vector x
# 初期状態ベクトル x
x = np.array([[0], [0]])  # Initial state / 初期状態

# Simulation settings
# シミュレーションのための設定
n_steps = 100  # Number of time steps / 時間ステップの数
v = np.random.normal(0, 1, (n_steps, 1))  # Input noise (system noise) / 入力雑音
y = np.zeros(n_steps)  # Initialize output / 出力の初期化

# Simulation loop
# シミュレーションループ
for k in range(n_steps):
    # Update state equation
    # 状態方程式の更新
    x = A @ x + B * v[k]
    
    # Output equation
    # 出力方程式
    y[k] = C @ x + D * v[k]

# Plot the results
# 結果をプロット
plt.figure(figsize=(10, 6))

# Plot the output y / 出力yのプロット
plt.plot(y, label='Output y')
plt.title('Output y Over Time (ARMA Model)')
plt.xlabel('Time Step')
plt.ylabel('Output')
plt.legend()

plt.tight_layout()
plt.show()
```

P33
```

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

# Define mean vector (mu) and covariance matrix (P)
# 平均ベクトル (mu) と共分散行列 (P) を定義
mu = np.array([0, 0])  # 平均ベクトル
P = np.array([[1.0, 0.5], [0.5, 1.0]])  # 共分散行列

# Create grid and multivariate normal distribution
# グリッドを作成し、多変量正規分布を定義
x1, x2 = np.mgrid[-3:3:.01, -3:3:.01]
pos = np.dstack((x1, x2))

# Calculate the probability density function
# 確率密度関数を計算
rv = multivariate_normal(mu, P)
z = rv.pdf(pos)

# Plot the 3D surface plot
# 3Dサーフェスプロットを描画
fig = plt.figure(figsize=(10, 7))
ax = fig.add_subplot(111, projection='3d')

# Plot the surface
# サーフェスをプロット
ax.plot_surface(x1, x2, z, cmap='viridis')

# Labels and title
# ラベルとタイトルを設定
ax.set_title('Multivariate Gaussian Distribution')
ax.set_xlabel('x1')
ax.set_ylabel('x2')
ax.set_zlabel('Probability Density')

# Show the plot
# プロットを表示
plt.show()
```

P39
```

import numpy as np
import matplotlib.pyplot as plt

# Generate synthetic data for y(k) using AR(2) model
# AR(2) モデルを使って合成データを生成
np.random.seed(0)
N = 100  # Number of data points / データポイントの数
a1 = 0.5  # True parameter for AR(1) term / AR(1)項の真のパラメータ
a2 = 0.3  # True parameter for AR(2) term / AR(2)項の真のパラメータ
v = np.random.normal(0, 1, N)  # Noise / ノイズ
y = np.zeros(N)

# Generate AR(2) data
# AR(2) データを生成
for k in range(2, N):
    y[k] = a1 * y[k-1] + a2 * y[k-2] + v[k]

# Prepare the regression matrix
# 回帰行列を準備
phi = np.zeros((N-2, 2))  # Regression matrix / 回帰行列
Y = y[2:]  # Dependent variable / 目的変数

for k in range(2, N):
    phi[k-2, :] = [y[k-1], y[k-2]]  # Regressors / 説明変数

# Solve the least squares problem
# 最小二乗法を使ってパラメータ推定
theta = np.linalg.inv(phi.T @ phi) @ (phi.T @ Y)

# Plot the results
# 結果をプロット
plt.figure(figsize=(10, 6))

# Plot true values vs estimated values / 真の値と推定値をプロット
plt.plot(y, label='True Data (y)')
plt.plot(range(2, N), phi @ theta, label='Estimated Data (Least Squares)', linestyle='--')

# Add labels and title / ラベルとタイトルを追加
plt.title('AR(2) Model: True vs Estimated Data')
plt.xlabel('Time Step')
plt.ylabel('y(k)')
plt.legend()

# Show the plot / プロットを表示
plt.show()

# Print the estimated parameters
# 推定されたパラメータを表示
print(f"Estimated parameters: a1 = {theta[0]:.4f}, a2 = {theta[1]:.4f}")
```

P42
```

import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import hankel, svd

# Step 1: Generate synthetic time series data using a known state-space model
# Step 1: 既知の状態空間モデルを使って時系列データを生成
np.random.seed(0)
n_samples = 100  # サンプル数
n_state = 2  # 状態数
A_true = np.array([[0.7, 0.1], [0.3, 0.9]])  # 真のシステム行列A
B_true = np.array([[0.1], [0.2]])  # 真の入力行列B
C_true = np.array([[1, 0]])  # 真の出力行列C

x = np.zeros((n_samples, n_state))  # 状態ベクトル
y = np.zeros(n_samples)  # 観測データ

# ノイズ
v = np.random.normal(0, 0.1, size=(n_samples, 1))  # システム雑音
w = np.random.normal(0, 0.1, size=n_samples)  # 観測雑音

# 初期状態
x[0] = [0.5, 0.5]

# システムをシミュレート
for k in range(1, n_samples):
    x[k] = A_true @ x[k-1] + B_true @ v[k]
    y[k] = C_true @ x[k] + w[k]

# Step 2: Compute the autocovariance matrix and construct the Hankel matrix
# Step 2: 自己共分散行列を計算し、ハンケル行列を構成する
h = 20  # Hankel matrix block size / ハンケル行列のブロックサイズ
H = hankel(y[:h], y[h-1:])

# Step 3: Singular Value Decomposition (SVD) of the Hankel matrix
# Step 3: ハンケル行列の特異値分解 (SVD)
U, S, Vh = svd(H)
S_diag = np.diag(S)

# Plot singular values to determine system order
# 特異値をプロットし、システムの次数を決定
plt.figure(figsize=(8, 6))
plt.plot(S, 'o-')
plt.title('Singular Values')
plt.xlabel('Index')
plt.ylabel('Singular Value')
plt.grid(True)
plt.show()

# Step 4: Estimate system matrices (A, B, C) using least squares
# Step 4: 最小二乗法を用いてシステム行列 (A, B, C) を推定する
n_order = 2  # システムの次数（特異値から決定）
U1 = U[:, :n_order]  # モード選択
S1 = S_diag[:n_order, :n_order]
V1 = Vh[:n_order, :]

# システム行列の推定
A_est = U1.T @ H[:, 1:] @ V1.T @ np.linalg.inv(S1)
C_est = U1[0, :]
B_est = np.linalg.pinv(U1.T @ H[:, :-1]) @ y[:h]

# 結果を表示
print("Estimated A matrix:\n", A_est)
print("Estimated B matrix:\n", B_est)
print("Estimated C matrix:\n", C_est)
```

P60
```

import numpy as np
import matplotlib.pyplot as plt

# Given parameters / 与えられたパラメータ
c = 0.8  # 変換係数
sigma_x = 1.0  # 信号 x の分散
sigma_w = 0.5  # 観測雑音 w の分散

# Generate synthetic data / 合成データの生成
N = 100  # サンプル数
true_x = 2.0  # 真の温度（信号）
w = np.random.normal(0, sigma_w, N)  # 観測雑音
y = c * true_x + w  # 観測値

# Step 1: Calculate the least-squares estimate
# ステップ1: 最小二乗推定値の計算
sigma_sq = 1 / (1/sigma_x**2 + (c**2 / sigma_w**2))  # 分散の推定
x_hat = (sigma_sq * c / sigma_w**2) * np.mean(y)  # 最小二乗推定値

# Step 2: Display results
# ステップ2: 結果を表示
print(f"True value of x: {true_x}")
print(f"Estimated value of x: {x_hat}")
print(f"Variance of the estimate: {sigma_sq}")

# Step 3: Plot the observations and estimate
# ステップ3: 観測値と推定値をプロット
plt.figure(figsize=(10, 6))

plt.plot(y, 'o', label='Observations (y)')
plt.axhline(y=true_x, color='r', linestyle='-', label='True Value of x')
plt.axhline(y=x_hat, color='g', linestyle='--', label='Estimated Value of x')

plt.title('Least-Squares Estimation of x from Observations')
plt.xlabel('Sample Index')
plt.ylabel('Observation / Estimate')
plt.legend()
plt.grid(True)
plt.show()
```

P69
```

import numpy as np
import matplotlib.pyplot as plt

# Parameters / パラメータの設定
n = 3  # 状態の次元数
m = 2  # 観測の次元数
p = 2  # 入力の次元数
N = 100  # サンプル数

# 真のシステム行列（既知のパラメータ） / True system matrices (known parameters)
A_true = np.array([[0.7, 0.1, 0.0], [0.3, 0.9, 0.0], [0.0, 0.0, 1.0]])
B_true = np.array([[0.1, 0.0], [0.0, 0.2], [0.1, 0.1]])
C_true = np.array([[1, 0, 0], [0, 1, 0]])

# システム雑音と観測雑音 / System noise and observation noise
w = np.random.normal(0, 0.1, (p, N))
v = np.random.normal(0, 0.1, (m, N))

# 初期状態ベクトル / Initial state vector
x = np.zeros((n, N))
y = np.zeros((m, N))

# 初期状態の設定 / Set initial state
x[:, 0] = np.random.rand(n)

# 観測データを生成 / Generate observation data
for k in range(1, N):
    x[:, k] = A_true @ x[:, k-1] + B_true @ w[:, k]
    y[:, k] = C_true @ x[:, k] + v[:, k]

# 最小二乗推定法の実行 / Apply least-squares estimation
Phi = np.zeros((m * N, n + p))  # 回帰行列 / Regression matrix
Y = y.T.flatten()  # 観測データをベクトル化 / Flattened observation data

for k in range(N):
    row_start = m * k
    row_end = row_start + m
    Phi[row_start:row_end, :n] = C_true @ np.linalg.matrix_power(A_true, k)
    Phi[row_start:row_end, n:] = C_true @ B_true

# 最小二乗推定を解く / Solve the least-squares problem
theta_est = np.linalg.lstsq(Phi, Y, rcond=None)[0]
A_est = theta_est[:n].reshape(n, n)
B_est = theta_est[n:].reshape(n, p)

# 結果の表示 / Display the results
print("True A matrix:\n", A_true)
print("Estimated A matrix:\n", A_est)
print("\nTrue B matrix:\n", B_true)
print("Estimated B matrix:\n", B_est)

# プロット / Plot the results
plt.figure(figsize=(10, 6))
plt.plot(y[0, :], label='True Observation y[0]')
plt.plot(Phi @ theta_est, label='Estimated Observation y[0]', linestyle='--')
plt.title('True vs Estimated Observation')
plt.xlabel('Time Step')
plt.ylabel('Observation Value')
plt.legend()
plt.grid(True)
plt.show()
```

P72
```

import numpy as np

# Parameters
n = 3  # 状態の次元数 (number of state variables)
m = 2  # 観測の次元数 (number of observation variables)
p = 2  # 入力の次元数 (number of inputs)
N = 100  # サンプル数 (number of samples)

# 真のシステム行列 (True system matrices)
A_true = np.array([[0.7, 0.1, 0.0], [0.3, 0.9, 0.0], [0.0, 0.0, 1.0]])  # 状態行列 (State transition matrix)
B_true = np.array([[0.1, 0.0], [0.0, 0.2], [0.1, 0.1]])  # 入力行列 (Input matrix)
C_true = np.array([[1, 0, 0], [0, 1, 0]])  # 観測行列 (Observation matrix)

# システム雑音と観測雑音 (System noise and observation noise)
w = np.random.normal(0, 0.1, (p, N))  # 入力ノイズ (Input noise)
v = np.random.normal(0, 0.1, (m, N))  # 観測ノイズ (Observation noise)

# 初期状態ベクトル (Initial state vector)
x = np.zeros((n, N))
y = np.zeros((m, N))

# 初期状態の設定 (Set initial state)
x[:, 0] = np.random.rand(n)

# 観測データを生成 (Generate observation data)
for k in range(1, N):
    x[:, k] = A_true @ x[:, k-1] + B_true @ w[:, k]  # 状態更新 (State update)
    y[:, k] = C_true @ x[:, k] + v[:, k]  # 観測 (Observation)

# 共分散行列の設定 (Covariance matrices)
Sigma_x = np.eye(n)  # 状態の共分散行列 (State covariance matrix)
Sigma_w = np.eye(m)  # 観測雑音の共分散行列 (Observation noise covariance matrix)

# F行列を計算 (Compute matrix F)
F = Sigma_x @ C_true.T @ np.linalg.inv(C_true @ Sigma_x @ C_true.T + Sigma_w)

# 推定された状態 (Estimated state)
x_hat = np.zeros((n, N))  # 初期化 (Initialize)

# 観測 y に基づいて推定を行う (Estimate based on observation y)
for k in range(N):
    x_hat[:, k] = F @ y[:, k]  # 最小二乗推定 (Least squares estimate)

# 結果の表示 (Display results)
print("True State x[:,0]:\n", x[:, 0])
print("Estimated State x_hat[:,0]:\n", x_hat[:, 0])

# 観測と推定結果をプロット (Plot the observations and estimated state)
import matplotlib.pyplot as plt

plt.figure(figsize=(10, 6))

# 観測値と推定値の比較 (Comparison of observations and estimates)
plt.plot(y[0, :], label="Observation y[0]")
plt.plot(x_hat[0, :], label="Estimated State x_hat[0]", linestyle="--")

plt.title("Least Squares Estimation of State Variables")
plt.xlabel("Time Step")
plt.ylabel("Value")
plt.legend()
plt.grid(True)
plt.show()
```

P79
```

import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm

# 観測データの生成 / Generate observation data
np.random.seed(0)
true_mean = 1.0  # 真の平均 / True mean
true_sigma = 1.0  # 真の標準偏差 / True standard deviation
N = 50  # 観測数 / Number of observations
y = np.random.normal(true_mean, true_sigma, N)  # 観測値 / Observed data

# 事前分布 / Prior distribution
prior_mean = 0.0  # 事前の平均 / Prior mean
prior_sigma = 1.0  # 事前の標準偏差 / Prior standard deviation

# 尤度関数 / Likelihood function (正規分布の場合)
def likelihood(mu, data):
    return np.prod(norm.pdf(data, mu, true_sigma))

# 事後分布 / Posterior distribution
mu_values = np.linspace(-3, 3, 1000)  # パラメータの候補値 / Candidate values for the parameter
posterior = []

for mu in mu_values:
    # ベイズの定理に従って事後確率を計算 / Calculate the posterior probability using Bayes' theorem
    prior = norm.pdf(mu, prior_mean, prior_sigma)  # 事前分布 / Prior probability
    likelihood_val = likelihood(mu, y)  # 尤度 / Likelihood
    posterior.append(prior * likelihood_val)

# 正規化 / Normalize the posterior
posterior = np.array(posterior)
posterior /= np.sum(posterior)  # Sum to 1

# 事後期待値 / Posterior mean
posterior_mean = np.sum(mu_values * posterior)

# 結果を表示 / Display the results
print(f"True mean: {true_mean}")
print(f"Posterior mean (estimate): {posterior_mean}")

# プロット / Plot the posterior distribution
plt.figure(figsize=(10, 6))
plt.plot(mu_values, posterior, label='Posterior Distribution')
plt.axvline(posterior_mean, color='r', linestyle='--', label=f'Posterior Mean: {posterior_mean:.2f}')
plt.axvline(true_mean, color='g', linestyle='-', label=f'True Mean: {true_mean:.2f}')
plt.title('Posterior Distribution of the Parameter')
plt.xlabel('Parameter (mu)')
plt.ylabel('Probability Density')
plt.legend()
plt.grid(True)
plt.show()
```

P95
```

import numpy as np
import matplotlib.pyplot as plt

# Step 1: モデルの設定（状態空間モデルの構築）
# Define system parameters (state-space model)
A = np.array([[1, 1], [0, 1]])  # 状態遷移行列 (State transition matrix)
B = np.array([[0.5], [1.0]])    # 制御行列 (Control matrix)
C = np.array([[1, 0]])          # 観測行列 (Observation matrix)
Q = np.array([[0.01, 0], [0, 0.01]])  # システム雑音の共分散 (Process noise covariance)
R = np.array([[0.1]])                 # 観測雑音の共分散 (Measurement noise covariance)
x_true = np.array([[0], [1]])         # 真の初期状態 (True initial state)

# シミュレーションの設定 (Simulation settings)
N = 50  # 時間ステップの数 (Number of time steps)
u = np.ones((N, 1))  # 定常制御入力 (Constant control input)
w = np.random.multivariate_normal([0, 0], Q, N).T  # システム雑音 (Process noise)
v = np.random.normal(0, np.sqrt(R[0][0]), N)       # 観測雑音 (Measurement noise)

# 真の状態と観測値の生成 (Generate true states and observations)
x_true_history = np.zeros((2, N))
y = np.zeros(N)

for k in range(1, N):
    x_true = A @ x_true + B @ u[k] + w[:, k:k+1]  # 真の状態遷移 (True state update)
    x_true_history[:, k] = x_true[:, 0]
    y[k] = C @ x_true + v[k]  # 観測 (Observation)

# Step 2: カルマンフィルタによる状態推定 (Kalman filter for state estimation)
x_est = np.zeros((2, N))  # 推定された状態 (Estimated states)
P = np.eye(2)  # 状態推定の誤差共分散行列 (Error covariance matrix)
x_est[:, 0] = [0, 0]  # 初期推定状態 (Initial estimated state)

# カルマンフィルタの実行 (Run Kalman filter)
for k in range(1, N):
    # 予測ステップ (Prediction step)
    x_pred = A @ x_est[:, k-1:k] + B @ u[k]  # 状態の予測 (State prediction)
    P_pred = A @ P @ A.T + Q  # 誤差共分散の予測 (Covariance prediction)

    # 更新ステップ (Update step)
    K = P_pred @ C.T @ np.linalg.inv(C @ P_pred @ C.T + R)  # カルマンゲインの計算 (Kalman gain)
    x_est[:, k:k+1] = x_pred + K @ (y[k] - C @ x_pred)  # 状態の更新 (State update)
    P = (np.eye(2) - K @ C) @ P_pred  # 誤差共分散の更新 (Covariance update)

# 結果をプロット (Plot the results)
plt.figure(figsize=(10, 6))
plt.plot(x_true_history[0, :], label='True Position')
plt.plot(x_est[0, :], label='Estimated Position', linestyle='--')
plt.plot(y, label='Observations', linestyle=':', color='gray')
plt.title('Kalman Filter State Estimation')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.grid(True)
plt.show()
```

P96
```

import numpy as np
import matplotlib.pyplot as plt

# モデルパラメータの設定 (Define model parameters)
A = np.array([[1.0, 1.0], [0, 1.0]])  # 状態遷移行列 (State transition matrix)
b = np.array([[0.5], [1.0]])  # 制御行列 (Control matrix)
c = np.array([[1.0, 0.0]])  # 観測行列 (Observation matrix)
Q = np.array([[0.01, 0.0], [0.0, 0.01]])  # システム雑音共分散 (Process noise covariance)
R = np.array([[0.1]])  # 観測雑音共分散 (Measurement noise covariance)

# 初期状態 (Initial state)
x_true = np.array([[0.0], [1.0]])  # 真の状態 (True state)
x_est = np.array([[0.0], [0.0]])  # 推定された状態 (Estimated state)
P = np.eye(2)  # 誤差共分散行列 (Error covariance)

# シミュレーションの設定 (Simulation settings)
N = 50  # サンプル数 (Number of time steps)
u = np.ones((N, 1))  # 制御入力 (Control input)
w = np.random.multivariate_normal([0, 0], Q, N).T  # システム雑音 (Process noise)
v = np.random.normal(0, np.sqrt(R[0][0]), N)  # 観測雑音 (Measurement noise)

# 真の状態と観測値の履歴 (True states and observations)
x_true_history = np.zeros((2, N))
y = np.zeros(N)

# 真の状態と観測値を生成 (Generate true states and observations)
for k in range(1, N):
    x_true = A @ x_true + b @ u[k] + w[:, k:k+1]  # 状態更新 (State update)
    x_true_history[:, k] = x_true[:, 0]
    y[k] = c @ x_true + v[k]  # 観測 (Observation)

# カルマンフィルタの実行 (Run Kalman filter)
x_est_history = np.zeros((2, N))  # 推定された状態の履歴 (Estimated state history)
x_est_history[:, 0] = x_est[:, 0]

for k in range(1, N):
    # 予測ステップ (Prediction step)
    x_pred = A @ x_est + b @ u[k]  # 状態の予測 (State prediction)
    P_pred = A @ P @ A.T + Q  # 誤差共分散の予測 (Covariance prediction)

    # カルマンゲインの計算 (Compute Kalman gain)
    K = P_pred @ c.T @ np.linalg.inv(c @ P_pred @ c.T + R)

    # 状態の更新 (State update)
    x_est = x_pred + K @ (y[k] - c @ x_pred)  # フィルタリングステップ (Filtering step)
    P = (np.eye(2) - K @ c) @ P_pred  # 共分散行列の更新 (Covariance update)

    # 推定結果を保存 (Save the estimated state)
    x_est_history[:, k] = x_est[:, 0]

# 結果をプロット (Plot the results)
plt.figure(figsize=(10, 6))
plt.plot(x_true_history[0, :], label='True Position')
plt.plot(x_est_history[0, :], label='Estimated Position', linestyle='--')
plt.plot(y, label='Observations', linestyle=':', color='gray')
plt.title('Kalman Filter: True State, Estimated State, and Observations')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.grid(True)
plt.show()
```




P100
```

import numpy as np
import matplotlib.pyplot as plt

# モデルの設定 (State-space model parameters)
A = np.array([[1.0, 1.0], [0.0, 1.0]])  # 状態遷移行列 (State transition matrix)
b = np.array([[0.5], [1.0]])  # 制御行列 (Control matrix)
c = np.array([[1.0, 0.0]])  # 観測行列 (Observation matrix)
Q = np.array([[0.01, 0.0], [0.0, 0.01]])  # システム雑音共分散 (Process noise covariance)
R = np.array([[0.1]])  # 観測雑音共分散 (Measurement noise covariance)

# 初期状態 (Initial state)
x_true = np.array([[0.0], [1.0]])  # 真の初期状態 (True initial state)
x_est = np.array([[0.0], [0.0]])  # 推定された状態の初期化 (Initial estimated state)
P = np.eye(2)  # 誤差共分散行列の初期化 (Initial error covariance)

# シミュレーションの設定 (Simulation settings)
N = 50  # サンプル数 (Number of time steps)
u = np.ones((N, 1))  # 制御入力 (Control input)
w = np.random.multivariate_normal([0, 0], Q, N).T  # システム雑音 (Process noise)
v = np.random.normal(0, np.sqrt(R[0][0]), N)  # 観測雑音 (Measurement noise)

# 真の状態と観測値の履歴 (True state and observations)
x_true_history = np.zeros((2, N))
y = np.zeros(N)

# 真の状態と観測値を生成 (Generate true states and observations)
for k in range(1, N):
    x_true = A @ x_true + b @ u[k] + w[:, k:k+1]  # 状態更新 (True state update)
    x_true_history[:, k] = x_true[:, 0]
    y[k] = c @ x_true + v[k]  # 観測 (Observation)

# カルマンフィルタの実行 (Run Kalman filter)
x_est_history = np.zeros((2, N))  # 推定された状態の履歴 (Estimated state history)
x_est_history[:, 0] = x_est[:, 0]

for k in range(1, N):
    # 予測ステップ (Prediction step)
    x_pred = A @ x_est + b @ u[k]  # 事前推定値 (a priori estimate)
    P_pred = A @ P @ A.T + Q  # 誤差共分散の予測 (Covariance prediction)

    # カルマンゲインの計算 (Compute Kalman gain)
    K = P_pred @ c.T @ np.linalg.inv(c @ P_pred @ c.T + R)

    # 状態の更新 (Update step)
    x_est = x_pred + K @ (y[k] - c @ x_pred)  # 事後推定値 (a posteriori estimate)
    P = (np.eye(2) - K @ c) @ P_pred  # 共分散行列の更新 (Covariance update)

    # 推定された状態を保存 (Save the estimated state)
    x_est_history[:, k] = x_est[:, 0]

# 結果をプロット (Plot the results)
plt.figure(figsize=(10, 6))
plt.plot(x_true_history[0, :], label='True Position')
plt.plot(x_est_history[0, :], label='Estimated Position', linestyle='--')
plt.plot(y, label='Observations', linestyle=':', color='gray')
plt.title('Kalman Filter: True State, Estimated State, and Observations')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.grid(True)
plt.show()
```

P104
```

import numpy as np
import matplotlib.pyplot as plt

# カルマンフィルタの設定 (Kalman Filter Parameters)
A = np.array([[1.0, 1.0], [0.0, 1.0]])  # 状態遷移行列 (State transition matrix)
b = np.array([[0.5], [1.0]])  # 制御行列 (Control matrix)
c = np.array([[1.0, 0.0]])  # 観測行列 (Observation matrix)
Q = np.array([[0.01, 0.0], [0.0, 0.01]])  # システム雑音共分散 (Process noise covariance)
R = np.array([[0.1]])  # 観測雑音共分散 (Measurement noise covariance)

# 初期状態と共分散 (Initial state and covariance)
x_true = np.array([[0.0], [1.0]])  # 真の初期状態 (True initial state)
x_est = np.array([[0.0], [0.0]])  # 推定された状態の初期化 (Initial estimated state)
P = np.eye(2)  # 誤差共分散行列の初期化 (Initial error covariance)

# シミュレーションの設定 (Simulation settings)
N = 50  # サンプル数 (Number of time steps)
u = np.ones((N, 1))  # 制御入力 (Control input)
w = np.random.multivariate_normal([0, 0], Q, N).T  # システム雑音 (Process noise)
v = np.random.normal(0, np.sqrt(R[0][0]), N)  # 観測雑音 (Measurement noise)

# 真の状態と観測値の履歴 (True state and observations)
x_true_history = np.zeros((2, N))
y = np.zeros(N)

# 真の状態と観測値を生成 (Generate true states and observations)
for k in range(1, N):
    x_true = A @ x_true + b @ u[k] + w[:, k:k+1]  # 状態更新 (True state update)
    x_true_history[:, k] = x_true[:, 0]
    y[k] = c @ x_true + v[k]  # 観測 (Observation)

# カルマンフィルタの実行 (Run Kalman filter)
x_est_history = np.zeros((2, N))  # 推定された状態の履歴 (Estimated state history)
x_est_history[:, 0] = x_est[:, 0]

for k in range(1, N):
    # Step 1: 予測ステップ (Prediction step)
    x_pred = A @ x_est + b @ u[k]  # 事前推定値 (a priori estimate)
    P_pred = A @ P @ A.T + Q  # 事前共分散行列 (Predicted covariance)

    # Step 2: カルマンゲインの計算 (Calculate Kalman Gain)
    K = P_pred @ c.T @ np.linalg.inv(c @ P_pred @ c.T + R)  # カルマンゲイン (Kalman Gain)

    # Step 3: 状態の更新 (Update step)
    x_est = x_pred + K @ (y[k] - c @ x_pred)  # 事後推定値 (a posteriori estimate)
    P = (np.eye(2) - K @ c) @ P_pred  # 事後共分散行列 (Updated covariance)

    # 推定された状態を保存 (Save the estimated state)
    x_est_history[:, k] = x_est[:, 0]

# 結果をプロット (Plot the results)
plt.figure(figsize=(10, 6))
plt.plot(x_true_history[0, :], label='True Position')
plt.plot(x_est_history[0, :], label='Estimated Position', linestyle='--')
plt.plot(y, label='Observations', linestyle=':', color='gray')
plt.title('Kalman Filter: True State, Estimated State, and Observations')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.grid(True)
plt.show()

```

P114
```

import numpy as np
import matplotlib.pyplot as plt

# カルマンフィルタの設定 (Kalman Filter Parameters)
A = np.array([[1]])  # 状態遷移行列 (State transition matrix)
B = np.array([[1]])  # システム雑音入力行列 (Process noise input matrix)
C = np.array([[1]])  # 観測行列 (Observation matrix)
Q = np.array([[1]])  # システム雑音共分散 (Process noise covariance)
R = np.array([[2]])  # 観測雑音共分散 (Measurement noise covariance)

# 初期状態と共分散 (Initial state and covariance)
x_est = np.array([[0]])  # 状態推定値の初期化 (Initial estimated state)
P = np.array([[0]])  # 誤差共分散行列の初期化 (Initial error covariance)

# シミュレーションの設定 (Simulation settings)
N = 4  # サンプル数 (Number of time steps)
v = np.random.normal(0, np.sqrt(Q[0][0]), N)  # システム雑音 (Process noise)
w = np.random.normal(0, np.sqrt(R[0][0]), N)  # 観測雑音 (Measurement noise)
y = np.zeros(N)  # 観測値の履歴 (Observation history)
x_true = np.zeros(N)  # 真の状態の履歴 (True state history)

# 状態と観測値の初期化 (Initial values for state and observation)
x_true[0] = 0
y[0] = x_true[0] + w[0]

# 状態と観測値の生成 (Generate true states and observations)
for k in range(1, N):
    x_true[k] = x_true[k - 1] + v[k - 1]  # 真の状態の更新 (True state update)
    y[k] = x_true[k] + w[k]  # 観測値の生成 (Generate observation)

# カルマンフィルタの実行 (Run Kalman filter)
x_est_history = np.zeros(N)  # 推定された状態の履歴 (Estimated state history)
x_est_history[0] = x_est  # 初期推定値を保存 (Store the initial estimate)

for k in range(1, N):
    # Step 1: 予測ステップ (Prediction step)
    x_pred = A @ x_est  # 事前推定値 (a priori estimate)
    P_pred = A @ P @ A.T + B @ Q @ B.T  # 事前共分散行列 (Predicted covariance)

    # Step 2: カルマンゲインの計算 (Calculate Kalman Gain)
    K = P_pred @ C.T @ np.linalg.inv(C @ P_pred @ C.T + R)  # カルマンゲイン (Kalman Gain)

    # Step 3: 状態の更新 (Update step)
    x_est = x_pred + K @ (y[k] - C @ x_pred)  # 事後推定値 (a posteriori estimate)
    P = (np.eye(1) - K @ C) @ P_pred  # 事後共分散行列 (Updated covariance)

    # 推定された状態を保存 (Save the estimated state)
    x_est_history[k] = x_est

# 結果をプロット (Plot the results)
plt.figure(figsize=(10, 6))
plt.plot(x_true, label='True State')
plt.plot(x_est_history, label='Estimated State', linestyle='--')
plt.plot(y, label='Observations', linestyle=':', color='gray')
plt.title('Kalman Filter: True State, Estimated State, and Observations (Multivariate)')
plt.xlabel('Time Step')
plt.ylabel('State Value')
plt.legend()
plt.grid(True)
plt.show()
```

P120
```

import numpy as np
import matplotlib.pyplot as plt

# Kalman Filter Implementation for 1D time series

# Parameters initialization
A = 1  # State transition matrix (in this case, a scalar)
B = 0  # Control matrix (assuming no control input here)
C = 1  # Measurement matrix (relating state to measurement)
Q = 1  # Process noise covariance
R = 2  # Measurement noise covariance
initial_state = 0  # Initial estimate of the state
initial_covariance = 1  # Initial covariance estimate

# Time series length
N = 50

# Simulate some true states and noisy observations
np.random.seed(42)
true_state = np.zeros(N)
observations = np.zeros(N)
true_state[0] = 10  # Initial true state
for k in range(1, N):
    true_state[k] = A * true_state[k - 1] + np.random.normal(0, np.sqrt(Q))  # True state evolution
    observations[k] = C * true_state[k] + np.random.normal(0, np.sqrt(R))  # Noisy measurements

# Kalman filter variables
state_estimates = np.zeros(N)
state_estimates[0] = initial_state
covariances = np.zeros(N)
covariances[0] = initial_covariance

for k in range(1, N):
    # Time update (prediction)
    state_predict = A * state_estimates[k - 1] + B  # No control input, B = 0
    covariance_predict = A * covariances[k - 1] * A + Q

    # Measurement update (correction)
    kalman_gain = covariance_predict * C / (C * covariance_predict * C + R)
    state_estimates[k] = state_predict + kalman_gain * (observations[k] - C * state_predict)
    covariances[k] = (1 - kalman_gain * C) * covariance_predict

# Plotting the results
plt.figure(figsize=(10, 6))
plt.plot(true_state, label="True State")
plt.plot(observations, label="Observations", linestyle='dashed', color='orange')
plt.plot(state_estimates, label="Kalman Filter Estimate", linestyle='dotted', color='green')
plt.title('Kalman Filter State Estimation')
plt.xlabel('Time Step')
plt.ylabel('State')
plt.legend()
plt.grid(True)
plt.show()
```

P122
```

import numpy as np

# Time steps
N = 10  # Number of time steps

# System matrices
A = np.array([[1, 0], [0, 1]])  # State transition matrix
B = np.array([[0.5], [0.1]])    # Control input matrix
C = np.array([[0, 1]])          # Observation matrix
Q = np.array([[0.01, 0], [0, 0.01]])  # System noise covariance
R = 0.1  # Observation noise covariance
P = np.eye(2)  # Initial error covariance matrix
x = np.zeros((2, 1))  # Initial state
x_hat = np.zeros((2, 1))  # Initial estimate
u = np.random.choice([-1, 1], N)  # Random control inputs
y = np.zeros((N, 1))  # Observations

# Kalman filter storage arrays
x_hat_arr = np.zeros((2, N))  # Filtered state estimates
P_arr = np.zeros((2, 2, N))  # Error covariance matrices
G_arr = np.zeros((2, N))  # Kalman gains

# Simulation (Assume system noise and observation noise as white Gaussian noise)
for k in range(1, N):
    # System model
    x = A @ x + B * u[k-1] + np.random.multivariate_normal([0, 0], Q).reshape(-1, 1)
    y[k] = C @ x + np.random.normal(0, R)
    
    # Prediction step
    x_hat_minus = A @ x_hat + B * u[k-1]
    P_minus = A @ P @ A.T + Q
    
    # Kalman gain calculation
    G = P_minus @ C.T @ np.linalg.inv(C @ P_minus @ C.T + R)
    
    # Update step
    x_hat = x_hat_minus + G @ (y[k] - C @ x_hat_minus)
    P = (np.eye(2) - G @ C) @ P_minus
    
    # Store results
    x_hat_arr[:, k] = x_hat.reshape(-1)
    P_arr[:, :, k] = P
    G_arr[:, k] = G.reshape(-1)

# Output results
print("State Estimates (x_hat):")
print(x_hat_arr)
print("Kalman Gains (G):")
print(G_arr)

P127

import numpy as np

# Kalman Filter Initialization
def kalman_filter(y, A, B, C, Q, R, x0, P0):
    n_timesteps = len(y)
    n = len(A)  # State dimension

    # Pre-allocate space for arrays
    x_hat = np.zeros((n_timesteps, n))  # Estimated state
    P = np.zeros((n_timesteps, n, n))   # Error covariance matrix
    x_hat_minus = np.zeros((n_timesteps, n))  # Prior estimated state
    P_minus = np.zeros((n_timesteps, n, n))   # Prior error covariance matrix
    K = np.zeros((n_timesteps, n))  # Kalman gain

    # Initial conditions
    x_hat[0] = x0
    P[0] = P0

    for k in range(1, n_timesteps):
        # Prediction step (time update)
        x_hat_minus[k] = np.dot(A, x_hat[k-1]) + B
        P_minus[k] = np.dot(A, np.dot(P[k-1], A.T)) + Q

        # Measurement update (correction step)
        S = np.dot(C, np.dot(P_minus[k], C.T)) + R  # Measurement prediction covariance
        K[k] = np.dot(P_minus[k], C.T) / S  # Kalman gain

        x_hat[k] = x_hat_minus[k] + K[k] * (y[k] - np.dot(C, x_hat_minus[k]))
        P[k] = P_minus[k] - np.dot(K[k], np.dot(C, P_minus[k]))

    return x_hat, P, K

# Example of usage
# Define system parameters (for a simple system)
A = np.array([[1]])  # State transition matrix
B = np.array([[0]])  # Control matrix
C = np.array([[1]])  # Observation matrix
Q = np.array([[1]])  # Process noise covariance
R = np.array([[2]])  # Measurement noise covariance
x0 = np.array([0])   # Initial state estimate
P0 = np.array([[1]]) # Initial estimate covariance

# Simulated measurements (for testing)
y = np.array([1, 2, 3, 2.5, 2, 1.5, 1, 0.5, 0])

# Run Kalman filter
x_hat, P, K = kalman_filter(y, A, B, C, Q, R, x0, P0)

# Print the results
for k in range(len(y)):
    print(f"Time step {k}: State estimate = {x_hat[k]}, Kalman Gain = {K[k]}")
```

P132
```

import numpy as np
import matplotlib.pyplot as plt
from scipy import signal

# Define the transfer function for the Kalman filter
# G(z) = 0.5 / (1 - 0.5z^-1)
numerator = [0.5]
denominator = [1, -0.5]

# Create a discrete-time transfer function
system = signal.dlti(numerator, denominator)

# Frequency range for plotting (0 to 0.5 Hz)
w, h = signal.dfreqresp(system, n=500)

# Convert from rad/sample to Hz (assuming sample rate = 1 Hz for simplicity)
freq = w / (2 * np.pi)

# Plot Gain in dB
plt.figure(figsize=(10, 6))

# Gain (dB)
plt.subplot(2, 1, 1)
plt.plot(freq, 20 * np.log10(abs(h)))
plt.title('Frequency Response of Kalman Filter')
plt.ylabel('Gain (dB)')
plt.grid(True)

# Phase (degrees)
plt.subplot(2, 1, 2)
plt.plot(freq, np.angle(h, deg=True))
plt.ylabel('Phase (degrees)')
plt.xlabel('Frequency (Hz)')
plt.grid(True)

# Show plots
plt.tight_layout()
plt.show()
```

P140
```

import numpy as np
import matplotlib.pyplot as plt

# シミュレーションの設定
np.random.seed(0)
N = 100  # データポイント数
true_params = np.array([0.5, -0.3])  # 真のパラメータ a1, a2
sigma_w = 1.0  # 観測ノイズの分散
gamma = 1000  # 共分散行列の初期値の定数

# 入力信号とノイズを生成
u = np.random.randn(N)
w = sigma_w * np.random.randn(N)  # ノイズ

# システムの出力 y を生成 (y(k) + a1*y(k-1) + a2*y(k-2) = b1*u(k-1) + b2*u(k-2) + w(k))
y = np.zeros(N)
for k in range(2, N):
    y[k] = true_params[0] * y[k-1] + true_params[1] * y[k-2] + u[k-1] + w[k]

# カルマンフィルタの初期設定
theta_hat = np.zeros((2, N))  # パラメータ推定値の初期化
P = gamma * np.eye(2)  # 共分散行列の初期化

# カルマンフィルタのアルゴリズム
for k in range(2, N):
    # 回帰ベクトルの定義
    phi_k = np.array([y[k-1], y[k-2]])
    
    # カルマンゲインの計算
    K = P @ phi_k / (phi_k.T @ P @ phi_k + sigma_w**2)
    
    # パラメータ推定値の更新
    theta_hat[:, k] = theta_hat[:, k-1] + K * (y[k] - phi_k.T @ theta_hat[:, k-1])
    
    # 共分散行列の更新
    P = P - np.outer(K, phi_k.T @ P)

# 結果のプロット
plt.figure(figsize=(10, 6))
plt.plot(range(N), theta_hat[0, :], label="Estimated a1")
plt.plot(range(N), theta_hat[1, :], label="Estimated a2")
plt.axhline(y=true_params[0], color='r', linestyle='--', label="True a1")
plt.axhline(y=true_params[1], color='g', linestyle='--', label="True a2")
plt.xlabel("Time step")
plt.ylabel("Parameter value")
plt.legend()
plt.title("Kalman Filter Parameter Estimation")
plt.grid(True)
plt.show()
```

P152
```

import numpy as np

# Function to compute the Jacobian matrix (example function, modify based on your system)
def jacobian_matrix(f, x):
    # Assuming f is a function of x, and x is the state vector
    # Use numerical differentiation to compute the Jacobian
    eps = 1e-5
    n = len(x)
    J = np.zeros((n, n))
    f0 = f(x)
    for i in range(n):
        x_eps = np.copy(x)
        x_eps[i] += eps
        J[:, i] = (f(x_eps) - f0) / eps
    return J

# Function f(x) representing the non-linear system dynamics
def f(x, u):
    # Example non-linear system: x_dot = Ax + Bu (modify for your system)
    A = np.array([[1, 1], [0, 1]])  # Modify based on the system
    B = np.array([[0.5], [1]])
    return A @ x + B @ u

# Function h(x) representing the measurement model
def h(x):
    # Example measurement model (modify for your system)
    return np.array([x[0]])

# Extended Kalman Filter function
def ekf_predict(x, P, u, Q, f, jacobian_f):
    # Prediction step: x = f(x, u), P = J_f * P * J_f.T + Q
    x = f(x, u)
    J_f = jacobian_f(f, x)  # Compute the Jacobian of f with respect to x
    P = J_f @ P @ J_f.T + Q
    return x, P

def ekf_update(x, P, z, R, h, jacobian_h):
    # Update step: y = z - h(x), S = H * P * H.T + R
    y = z - h(x)
    H = jacobian_h(h, x)  # Compute the Jacobian of h with respect to x
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)  # Kalman gain
    x = x + K @ y  # Update the state estimate
    P = (np.eye(len(x)) - K @ H) @ P  # Update the error covariance
    return x, P

# Initialization
x = np.array([0, 0])  # Initial state
P = np.eye(2) * 1  # Initial covariance matrix
Q = np.eye(2) * 0.1  # Process noise covariance
R = np.eye(1) * 0.1  # Measurement noise covariance
u = np.array([1])  # Control input (example)
z = np.array([0.5])  # Measurement (example)

# Simulate EKF for one step
x_pred, P_pred = ekf_predict(x, P, u, Q, f, jacobian_matrix)
x_update, P_update = ekf_update(x_pred, P_pred, z, R, h, jacobian_matrix)

# Output the results
print("Predicted State: ", x_pred)
print("Updated State: ", x_update)
print("Updated Covariance Matrix: ", P_update)
```

P155
```

import numpy as np

def linear_kf_predict(x, P, A, B, u, Q):
    # State prediction
    x = A @ x + B @ u
    # Covariance prediction
    P = A @ P @ A.T + Q
    return x, P

def linear_kf_update(x, P, z, H, R):
    # Measurement update
    y = z - H @ x  # Residual
    S = H @ P @ H.T + R  # Residual covariance
    K = P @ H.T @ np.linalg.inv(S)  # Kalman gain
    x = x + K @ y  # Updated state
    P = (np.eye(len(x)) - K @ H) @ P  # Updated covariance
    return x, P

# Initialize parameters
x = np.array([0, 0])  # Initial state
P = np.eye(2)  # Covariance matrix
A = np.array([[1, 1], [0, 1]])  # State transition matrix
B = np.array([[0.5], [1]])  # Control matrix
u = np.array([1])  # Control input
H = np.array([[1, 0]])  # Measurement matrix
R = np.eye(1) * 0.1  # Measurement noise covariance
Q = np.eye(2) * 0.1  # Process noise covariance
z = np.array([1])  # Measurement

# Apply Kalman Filter
x_pred, P_pred = linear_kf_predict(x, P, A, B, u, Q)
x_update, P_update = linear_kf_update(x_pred, P_pred, z, H, R)

print("KF Updated State: ", x_update)
print("KF Updated Covariance: ", P_update)
```

P156
```

import numpy as np
from scipy.linalg import cholesky

### 1. Linear Kalman Filter (KF)
def linear_kf_predict(x, P, A, B, u, Q):
    # State prediction
    x = A @ x + B @ u
    # Covariance prediction
    P = A @ P @ A.T + Q
    return x, P

def linear_kf_update(x, P, z, H, R):
    # Measurement update
    y = z - H @ x  # Residual
    S = H @ P @ H.T + R  # Residual covariance
    K = P @ H.T @ np.linalg.inv(S)  # Kalman gain
    x = x + K @ y  # Updated state
    P = (np.eye(len(x)) - K @ H) @ P  # Updated covariance
    return x, P

# Initial setup for the Kalman Filter
x = np.array([0, 0])  # Initial state
P = np.eye(2)  # Covariance matrix
A = np.array([[1, 1], [0, 1]])  # State transition matrix
B = np.array([[0.5], [1]])  # Control matrix
u = np.array([1])  # Control input
H = np.array([[1, 0]])  # Measurement matrix
R = np.eye(1) * 0.1  # Measurement noise covariance
Q = np.eye(2) * 0.1  # Process noise covariance
z = np.array([1])  # Measurement

# Apply Linear Kalman Filter
x_pred, P_pred = linear_kf_predict(x, P, A, B, u, Q)
x_update, P_update = linear_kf_update(x_pred, P_pred, z, H, R)

print("KF Updated State: ", x_update)
print("KF Updated Covariance: ", P_update)

### 2. Extended Kalman Filter (EKF)
def f_ekf(x, u):
    return np.array([x[0] + x[1] + u[0], x[1] + u[0]])

def h_ekf(x):
    return np.array([x[0]])

def jacobian_f_ekf(x, u):
    return np.array([[1, 1], [0, 1]])

def jacobian_h_ekf(x):
    return np.array([[1, 0]])

def ekf_predict(x, P, u, Q, f, jacobian_f):
    # State prediction
    x = f(x, u)
    J_f = jacobian_f(x, u)
    P = J_f @ P @ J_f.T + Q
    return x, P

def ekf_update(x, P, z, R, h, jacobian_h):
    # Measurement update
    y = z - h(x)
    J_h = jacobian_h(x)
    S = J_h @ P @ J_h.T + R
    K = P @ J_h.T @ np.linalg.inv(S)
    x = x + K @ y
    P = (np.eye(len(x)) - K @ J_h) @ P
    return x, P

# Apply Extended Kalman Filter
x_pred, P_pred = ekf_predict(x, P, u, Q, f_ekf, jacobian_f_ekf)
x_update, P_update = ekf_update(x_pred, P_pred, z, R, h_ekf, jacobian_h_ekf)

print("EKF Updated State: ", x_update)
print("EKF Updated Covariance: ", P_update)

### 3. Unscented Kalman Filter (UKF)
def sigma_points(x, P, kappa=1):
    n = len(x)
    sigma_pts = np.zeros((2 * n + 1, n))
    W = np.zeros(2 * n + 1)
    sigma_pts[0] = x
    W[0] = kappa / (n + kappa)
    U = cholesky((n + kappa) * P)
    
    for i in range(n):
        sigma_pts[i + 1] = x + U[i]
        sigma_pts[n + i + 1] = x - U[i]
        W[i + 1] = 1 / (2 * (n + kappa))
        W[n + i + 1] = 1 / (2 * (n + kappa))
    
    return sigma_pts, W

def ukf_predict(sigma_pts, W, f, Q):
    n = len(sigma_pts[0])
    x_pred = np.sum([W[i] * f(sigma_pts[i]) for i in range(2 * n + 1)], axis=0)
    P_pred = Q + np.sum([W[i] * np.outer(f(sigma_pts[i]) - x_pred, f(sigma_pts[i]) - x_pred) for i in range(2 * n + 1)], axis=0)
    return x_pred, P_pred

def ukf_update(sigma_pts, W, x_pred, P_pred, z, h, R):
    n = len(sigma_pts[0])
    z_pred = np.sum([W[i] * h(sigma_pts[i]) for i in range(2 * n + 1)], axis=0)
    P_zz = R + np.sum([W[i] * np.outer(h(sigma_pts[i]) - z_pred, h(sigma_pts[i]) - z_pred) for i in range(2 * n + 1)], axis=0)
    P_xz = np.sum([W[i] * np.outer(sigma_pts[i] - x_pred, h(sigma_pts[i]) - z_pred) for i in range(2 * n + 1)], axis=0)
    
    K = P_xz @ np.linalg.inv(P_zz)
    x_update = x_pred + K @ (z - z_pred)
    P_update = P_pred - K @ P_zz @ K.T
    return x_update, P_update

# Generate sigma points and apply Unscented Kalman Filter
sigma_pts, W = sigma_points(x, P)
x_pred, P_pred = ukf_predict(sigma_pts, W, f_ekf, Q)
x_update, P_update = ukf_update(sigma_pts, W, x_pred, P_pred, z, h_ekf, R)

print("UKF Updated State: ", x_update)
print("UKF Updated Covariance: ", P_update)

### 4. Particle Filter (PF)
def particle_filter_predict(particles, weights, f, u, Q):
    n_particles = len(particles)
    particles_pred = np.zeros_like(particles)
    
    for i in range(n_particles):
        particles_pred[i] = f(particles[i], u) + np.random.multivariate_normal([0, 0], Q)
    
    return particles_pred

def particle_filter_update(particles, weights, z, h, R):
    weights = np.array([np.exp(-0.5 * np.linalg.norm(z - h(p))**2 / R[0, 0]) for p in particles])
    weights /= np.sum(weights)
    return weights

def resample(particles, weights):
    indices = np.random.choice(len(particles), size=len(particles), p=weights)
    return particles[indices]

# Initialize particles and weights
n_particles = 100
particles = np.random.randn(n_particles, 2)
weights = np.ones(n_particles) / n_particles
Q = np.eye(2) * 0.1  # Process noise covariance
R = np.eye(1) * 0.1  # Measurement noise covariance
z = np.array([1])  # Measurement

# Particle Filter predict step
particles_pred = particle_filter_predict(particles, weights, f_ekf, u, Q)

# Particle Filter update step
weights = particle_filter_update(particles_pred, weights, z, h_ekf, R)

# Resampling step
particles_resampled = resample(particles_pred, weights)

print("Resampled Particles: ", particles_resampled)

```


P164
```

import numpy as np
import matplotlib.pyplot as plt

# Function for Sigma Point Generation using U Transformation
def sigma_points(x, P, kappa):
    """ 
    Japanese: シグマポイントを生成するための関数
    English: Function to generate sigma points using U transformation
    """
    n = len(x)  # Dimension of the state
    sigma_points = np.zeros((2 * n + 1, n))  # Initialize sigma points
    sigma_points[0] = x  # The first sigma point is the mean

    # Compute the square root of the covariance matrix
    sqrt_P = np.linalg.cholesky((n + kappa) * P)

    # Generate the remaining sigma points
    for i in range(n):
        sigma_points[i + 1] = x + sqrt_P[i]
        sigma_points[i + n + 1] = x - sqrt_P[i]

    return sigma_points

# Example system dynamics (linearized for simplicity)
def f(x):
    """ 
    Japanese: システムの状態更新関数
    English: System state update function 
    """
    return np.array([x[0] + x[1], x[1]])

def h(x):
    """ 
    Japanese: 観測関数
    English: Observation function
    """
    return np.array([x[0]])

# Example parameters
n = 2  # State dimension
x = np.array([1.0, 0.5])  # State mean
P = np.array([[1.0, 0.5], [0.5, 2.0]])  # State covariance matrix
kappa = 3 - n  # Scaling parameter

# Generate sigma points
sigma_pts = sigma_points(x, P, kappa)

# Plot Sigma Points
plt.figure()
plt.scatter(sigma_pts[:, 0], sigma_pts[:, 1], color='red', label='Sigma Points')
plt.scatter(x[0], x[1], color='blue', label='Mean')
plt.title('Sigma Points and Mean')
plt.xlabel('State Dimension 1')
plt.ylabel('State Dimension 2')
plt.legend()
plt.grid(True)
plt.show()

# Prediction step
predicted_sigma_pts = np.array([f(s) for s in sigma_pts])

# Compute predicted mean and covariance
w0 = kappa / (n + kappa)  # Weight for the mean
wi = 1 / (2 * (n + kappa))  # Weights for the other sigma points

# Predicted mean
predicted_mean = w0 * predicted_sigma_pts[0] + sum(wi * predicted_sigma_pts[i] for i in range(1, 2 * n + 1))

# Predicted covariance
predicted_cov = w0 * np.outer(predicted_sigma_pts[0] - predicted_mean, predicted_sigma_pts[0] - predicted_mean)
for i in range(1, 2 * n + 1):
    predicted_cov += wi * np.outer(predicted_sigma_pts[i] - predicted_mean, predicted_sigma_pts[i] - predicted_mean)

# Print results
print(f"Predicted mean: {predicted_mean}")
print(f"Predicted covariance:\n {predicted_cov}")
```

P167
```

import numpy as np

# Define the initial state (x_0)
x = np.array([[0], [0]])  # Initial state vector

# Define the state transition matrix (A), control matrix (B), and observation matrix (C)
A = np.array([[1, 1], [0, 1]])  # State transition model
B = np.array([[0.5], [1]])      # Control input model
C = np.array([[1, 0]])          # Observation model

# Define the process noise covariance (Q), observation noise covariance (R), and initial estimation error covariance (P)
Q = np.array([[0.1, 0], [0, 0.1]])  # Process noise covariance
R = np.array([[0.1]])               # Observation noise covariance
P = np.array([[1, 0], [0, 1]])      # Initial estimate error covariance

# Define control input (u) and measurements (y)
u = np.array([[2]])  # Control input
y = np.array([[1.1]])  # Measurement

# Kalman filter steps

# Prediction step
x_pred = A @ x + B @ u  # Predicted state estimate
P_pred = A @ P @ A.T + Q  # Predicted estimate covariance

# Update step (Correction step)
K = P_pred @ C.T @ np.linalg.inv(C @ P_pred @ C.T + R)  # Kalman gain
x = x_pred + K @ (y - C @ x_pred)  # Updated state estimate
P = (np.eye(2) - K @ C) @ P_pred  # Updated estimate covariance

# Print results
print("Updated state estimate: \n", x)
print("Updated estimate covariance: \n", P)
```

P172
```

import numpy as np

# Unscented Kalman Filter

def sigma_points(mu, cov, kappa):
    """
    Calculate the sigma points based on the mean (mu) and covariance (cov).
    """
    n = len(mu)
    sigma_points = np.zeros((2 * n + 1, n))
    weights = np.zeros(2 * n + 1)
    
    # Scaling parameter
    lambda_ = kappa + n
    
    # First sigma point is the mean
    sigma_points[0] = mu
    weights[0] = lambda_ / (n + lambda_)
    
    # Remaining sigma points
    sqrt_cov = np.linalg.cholesky((n + lambda_) * cov)
    
    for i in range(n):
        sigma_points[i + 1] = mu + sqrt_cov[:, i]
        sigma_points[n + i + 1] = mu - sqrt_cov[:, i]
        weights[i + 1] = 1 / (2 * (n + lambda_))
        weights[n + i + 1] = 1 / (2 * (n + lambda_))
    
    return sigma_points, weights

def unscented_transform(sigma_points, weights):
    """
    Perform the unscented transform to recover the mean and covariance.
    """
    mean = np.dot(weights, sigma_points)
    covariance = np.zeros((len(mean), len(mean)))
    
    for i in range(len(sigma_points)):
        diff = sigma_points[i] - mean
        covariance += weights[i] * np.outer(diff, diff)
    
    return mean, covariance

def predict_sigma_points(sigma_points, f):
    """
    Predict the sigma points based on the motion model.
    """
    return np.array([f(sigma) for sigma in sigma_points])

def unscented_kalman_filter(mu, cov, f, h, z, R, Q, kappa):
    """
    Unscented Kalman Filter for nonlinear state estimation.
    
    Parameters:
    mu   -- Prior mean
    cov  -- Prior covariance
    f    -- State transition function
    h    -- Measurement function
    z    -- Measurement
    R    -- Measurement noise covariance
    Q    -- Process noise covariance
    kappa -- Scaling parameter
    """
    # Generate sigma points
    sigma_points, weights = sigma_points(mu, cov, kappa)
    
    # Predict sigma points
    predicted_sigma_points = predict_sigma_points(sigma_points, f)
    
    # Perform unscented transform to recover predicted state
    mu_bar, cov_bar = unscented_transform(predicted_sigma_points, weights)
    cov_bar += Q  # Add process noise
    
    # Predict measurement
    predicted_measurements = np.array([h(sigma) for sigma in predicted_sigma_points])
    z_pred, S = unscented_transform(predicted_measurements, weights)
    S += R  # Add measurement noise
    
    # Cross-covariance
    P_xz = np.zeros((len(mu), len(z)))
    for i in range(len(sigma_points)):
        P_xz += weights[i] * np.outer(predicted_sigma_points[i] - mu_bar, predicted_measurements[i] - z_pred)
    
    # Kalman gain
    K = np.dot(P_xz, np.linalg.inv(S))
    
    # Update state mean and covariance
    mu_new = mu_bar + np.dot(K, (z - z_pred))
    cov_new = cov_bar - np.dot(np.dot(K, S), K.T)
    
    return mu_new, cov_new

# Example usage of UKF

def state_transition_function(x):
    # Example state transition function
    return x

def measurement_function(x):
    # Example measurement function
    return x

# Initial state
mu = np.array([0, 0])
cov = np.eye(2) * 0.1
R = np.eye(2) * 0.1  # Measurement noise
Q = np.eye(2) * 0.01  # Process noise
z = np.array([1.0, 1.0])  # Measurement
kappa = 0

# Run UKF
mu_new, cov_new = unscented_kalman_filter(mu, cov, state_transition_function, measurement_function, z, R, Q, kappa)

print("Updated State Mean: ", mu_new)
print("Updated Covariance: ", cov_new)
```



```




```