卡爾曼濾波器（Kalman Filter, KF）是一種最優估計技術，可用於根據角度測量來估算速度。在這裡，我們假設有一個系統，其中角度是可測量的，而速度是我們要估算的隱藏狀態。

---

## **問題定義**
我們考慮一個一維系統：
- 狀態向量：$x = \begin{bmatrix} \theta \\ \dot{\theta} \end{bmatrix}$（角度和角速度）
- 狀態轉移模型：
$$
x_{k+1} = A x_k + w_k
$$
其中：
$$
A = \begin{bmatrix} 1 & T \\ 0 & 1 \end{bmatrix}
$$
$T$ 是離散時間間隔，$w_k$ 是過程噪聲。
- 測量方程：
$$
z_k = H x_k + v_k
$$
其中：
$$
H = \begin{bmatrix} 1 & 0 \end{bmatrix}
$$
$z_k$ 是測得的角度，$v_k$ 是測量噪聲。

---

## **Matlab Code**
以下是使用卡爾曼濾波器來估計角速度的 Matlab 代碼：

clc; clear; close all;

% 參數設定
T = 0.1;  % 取樣時間
A = [1 T; 0 1];  % 狀態轉移矩陣
H = [1 0];  % 觀測矩陣
Q = [1e-4 0; 0 1e-2];  % 過程噪聲協方差
R = 1e-2;  % 測量噪聲協方差
P = eye(2);  % 初始協方差矩陣
x_est = [0; 0];  % 初始狀態 [theta; theta_dot]

% 生成模擬數據
N = 100;  % 時間步數
theta_true = zeros(1, N);
theta_dot_true = zeros(1, N);
z = zeros(1, N);

for k = 2:N
    theta_dot_true(k) = 0.1 * sin(0.1 * k);  % 真實速度變化
    theta_true(k) = theta_true(k-1) + T * theta_dot_true(k);
    z(k) = theta_true(k) + sqrt(R) * randn;  % 加入測量噪聲
end

% 卡爾曼濾波估計
x_est_hist = zeros(2, N);

for k = 1:N
    % 預測步驟
    x_pred = A * x_est;
    P_pred = A * P * A' + Q;
    
    % 更新步驟
    K = P_pred * H' / (H * P_pred * H' + R);
    x_est = x_pred + K * (z(k) - H * x_pred);
    P = (eye(2) - K * H) * P_pred;
    
    % 儲存結果
    x_est_hist(:, k) = x_est;
end

% 畫圖
figure;
subplot(2,1,1);
plot(1:N, theta_true, 'g', 1:N, z, 'b.', 1:N, x_est_hist(1,:), 'r');
legend('True Angle', 'Measured Angle', 'Estimated Angle');
xlabel('Time Step'); ylabel('Angle (rad)');

subplot(2,1,2);
plot(1:N, theta_dot_true, 'g', 1:N, x_est_hist(2,:), 'r');
legend('True Velocity', 'Estimated Velocity');
xlabel('Time Step'); ylabel('Angular Velocity (rad/s)');

---

## **代碼解釋**
1. **系統初始化**：
- 設定狀態轉移矩陣 $A$、觀測矩陣 $H$ 及協方差矩陣 $Q, R$。
- 初始化狀態 $x_{est}$ 和協方差 $P$。

2. **模擬真實數據**：
- 使用 $\theta_{dot}(k) = 0.1 \sin(0.1 k)$ 產生角速度變化。
- 通過數值積分獲得角度，並添加測量噪聲。

3. **卡爾曼濾波迴圈**：
- **預測步驟**：
- 計算預測狀態 $x_{pred} = A x_{est}$。
- 更新誤差協方差矩陣 $P_{pred}$。
- **更新步驟**：
- 計算卡爾曼增益 $K$。
- 更新狀態估計 $x_{est}$。
- 更新誤差協方差矩陣 $P$。

4. **繪圖**：
- 比較真實角度、測量角度與估計角度。
- 比較真實角速度與估計角速度。

這樣就能使用卡爾曼濾波器來估算角速度，並在有噪聲的情況下得到較準確的結果！

## 限制式
### 連續form
$$I\ddot{\alpha}<0.6$$$$I\ddot{\beta}<0.6$$
### Discrete form
$$I \frac{\alpha_{k+1} - 2\alpha_k + \alpha_{k-1}}{T^2} < 0.6$$$$I \frac{\beta_{k+1} - 2\beta_k + \beta_{k-1}}{T^2} < 0.6$$