针对正弦波采样的卡尔曼滤波算法，以下是详细的解释和实现步骤：

1. 应用场景

• 目标：对含噪声的正弦波信号进行实时滤波，估计其真实幅值、相位或瞬时值。
• 典型场景：传感器信号去噪（如振动检测、通信信号恢复）、参数跟踪（如幅值/频率估计）等。

2. 状态空间建模

状态向量定义

假设正弦波频率已知且固定，幅值恒定，相位随时间线性变化。将状态向量分解为正交的正弦和余弦分量： [ \mathbf{x}k = \begin{bmatrix} x{1,k} \ x_{2,k} \end{bmatrix} = \begin{bmatrix} A \sin(\theta_k) \ A \cos(\theta_k) \end{bmatrix} ] 其中：

• ( A ) 为幅值，( \theta_k = \omega k \Delta t + \phi )（频率 ( \omega )、采样间隔 ( \Delta t )、初始相位 ( \phi )）。

过程模型

利用旋转矩阵更新状态（相位随时间变化）： [ \mathbf{x}_{k+1} = \mathbf{F} \mathbf{x}_k + \mathbf{w}_k ] 其中：

• 状态转移矩阵： [ \mathbf{F} = \begin{bmatrix} \cos(\omega \Delta t) & -\sin(\omega \Delta t) \ \sin(\omega \Delta t) & \cos(\omega \Delta t) \end{bmatrix} ]
• ( \mathbf{w}_k ) 为过程噪声（协方差矩阵 ( \mathbf{Q} )）。

观测模型

假设观测值为正弦分量加噪声： [ z_k = \mathbf{H} \mathbf{x}_k + v_k, \quad \mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} ]

• ( v_k ) 为观测噪声（方差 ( R )）。

3. 卡尔曼滤波步骤

1. 初始化：

   • 初始状态估计 ( \hat{\mathbf{x}}_0 ) 和协方差矩阵 ( \mathbf{P}_0 )。

2. 预测： [ \hat{\mathbf{x}}^-k = \mathbf{F} \hat{\mathbf{x}}{k-1} ] [ \mathbf{P}^-k = \mathbf{F} \mathbf{P}{k-1} \mathbf{F}^T + \mathbf{Q} ]

3. 更新：

   • 计算卡尔曼增益： [ \mathbf{K}_k = \mathbf{P}^-_k \mathbf{H}^T \left( \mathbf{H} \mathbf{P}^-_k \mathbf{H}^T + R \right)^{-1} ]
   • 修正状态估计： [ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}^-_k + \mathbf{K}_k \left( z_k - \mathbf{H} \hat{\mathbf{x}}^-_k \right) ]
   • 更新协方差： [ \mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}) \mathbf{P}^-_k ]

4. 参数调整

• 过程噪声 ( \mathbf{Q} )：反映模型不确定性（如小幅频率漂移或幅值变化）。
• 观测噪声 ( R )：由传感器噪声特性决定。
• 初始状态：若未知相位，可设为 ( \hat{\mathbf{x}}_0 = [0, A]^T )，协方差 ( \mathbf{P}_0 ) 设为较大值表示初始不确定性。

5. 扩展场景（频率未知）

若频率 ( \omega ) 未知，需将其加入状态向量并使用 扩展卡尔曼滤波（EKF）：

• 状态向量：( \mathbf{x}k = [x{1,k}, x_{2,k}, \omega_k]^T )
• 非线性状态转移：需对模型线性化，计算雅可比矩阵更新协方差。

6. 示例代码框架（Python）

import numpy as np

# 参数设置
dt = 0.01       # 采样间隔
omega = 2*np.pi # 已知频率
Q = np.eye(2)*0.01  # 过程噪声协方差
R = 0.1         # 观测噪声方差

# 状态转移矩阵
F = np.array([[np.cos(omega*dt), -np.sin(omega*dt)],
              [np.sin(omega*dt), np.cos(omega*dt)]])

H = np.array([1, 0])          # 观测矩阵
x_hat = np.array([0, 1.0])    # 初始状态估计 [A*sin(θ0), A*cos(θ0)]
P = np.eye(2)                 # 初始协方差

for z_k in observations:
    # 预测
    x_hat_minus = F @ x_hat
    P_minus = F @ P @ F.T + Q

    # 更新
    K = P_minus @ H.T / (H @ P_minus @ H.T + R)
    x_hat = x_hat_minus + K * (z_k - H @ x_hat_minus)
    P = (np.eye(2) - K.reshape(-1,1) @ H.reshape(1,-1)) @ P_minus

    # 输出估计值（正弦分量）
    estimated_signal = H @ x_hat

7. 关键点总结

• 线性化技巧：通过正交分量建模，将非线性问题转化为线性状态空间。
• 实时性：适合嵌入式系统或实时信号处理。
• 调参核心：合理设置 ( \mathbf{Q} ) 和 ( R ) 以平衡响应速度与平滑度。

通过上述步骤，卡尔曼滤波可有效滤除正弦波中的噪声，同时跟踪其动态变化。