卡尔曼滤波使用手册

import numpy as np
import matplotlib.pyplot as plt

# 一维位置估计 - 基础卡尔曼滤波演示
# 一个人以0.2m/s恒定速度在一维空间中向前移动，观测他的位置50秒


# 给定参数
H = 1 # 观测矩阵
F = 1 # 状态转移矩阵
B = 0   # 控制输入矩阵
Q = 0.5 # 过程噪声协方差矩阵
R = 2.0 # 测量噪声协方差矩阵


# 指定理论值，方便评估精度，实际无法获得
true_values = []
for i in range(50):
    true_values.append(i*0.2)  # 真实位置为0到10之间
true_values = np.array(true_values, dtype=float)

# 系列观测与控制输入
np.random.seed(0)
z = true_values + np.random.normal(0, 1, size=len(true_values))
u = np.zeros_like(z)

# 给状态赋初值
prev_state = z[0]
prev_error = 1.0

# 用于存放估计的状态
predict_states = []

# 卡尔曼滤波过程
for i in range(len(z)):
    # 预测 - 状态
    predict_state = F * prev_state + B * u[i]

    # 预测 - 误差
    predict_error = F * prev_error * F + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain = (predict_error * H) / (H * predict_error * H + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain * (z[i] - H * predict_state)
    # 更新 - 误差
    update_error = (1 - kalman_gain * H) * predict_error
    
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error

    # 将预测状态保存起来
    predict_states.append(update_state)


# 精度评价
error_measure = np.mean(np.abs(z  - true_values))
error_kalman = np.mean(np.abs(predict_states - true_values))

print("Mean Error of Measurements:", error_measure)
print("Mean Error of Kalman Filter:", error_kalman)

# 可视化结果
plt.figure(figsize=(10,6))
plt.plot(true_values, np.zeros_like(true_values), label='True Values', linestyle='--')
plt.plot(true_values, z - true_values, label='Noisy Measurements', marker='o', markersize=4)
plt.plot(true_values, predict_states - true_values, label='Kalman Filter', linewidth=2)
plt.legend()
plt.xlabel('Time Step')
plt.ylabel('Value')
plt.title('Basic Kalman Filter Demo')
plt.show()

import numpy as np

# 给定参数
H = 0.5 # 观测矩阵
F = 1.0 # 状态转移矩阵
B = 100.0   # 控制输入矩阵
Q = 0.1 # 过程噪声协方差矩阵
R = 0.5 # 测量噪声协方差矩阵

# 指定理论值，方便评估精度，实际无法获得
ideal_value = np.zeros(12) + 200

# 系列观测与控制输入
z = [102, 104, 98, 99.7, 102, 101.5, 100.4, 101.6, 103.8, 97, 102.1, 99.3]
u = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

# 给状态赋初值
prev_state = z[0] / H
prev_error = 1.0

# 用于存放估计的状态
predict_states = []
measure_states = []

# 开始卡尔曼滤波
for i in range(len(z)):
    # 预测 - 状态
    predict_state = F * prev_state + B * u[i]
    # 预测 - 误差
    predict_error = F * prev_error * F + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain  = (predict_error * H) / (H * predict_error * H + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain * (z[i] - H * predict_state)
    # 更新 - 误差
    update_error = (1 - kalman_gain * H) * predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state)
    measure_states.append(z[i] / H)

# 精度评价
measure_errors = np.abs(measure_states - ideal_value)
predict_errors = np.abs(predict_states - ideal_value)

for i in range(len(predict_errors)):
    print(i+1, "/", len(predict_errors), "predict error:", round(predict_errors[i], 5), "measure error:", round(measure_errors[i], 5))

print("mean measure", round(np.mean(measure_errors), 5))
print("mean predict", round(np.mean(predict_errors), 5))

import numpy as np
from matplotlib import pyplot as plt

# 给定参数
# 两个状态间的时间间隔
delta_t = 1

# 观测矩阵
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])
# 状态转移矩阵
F = np.array([[1, 0, delta_t, 0],
              [0, 1, 0, delta_t],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])
# 控制输入矩阵
B = np.array([[0, 0],
              [0, 0],
              [10, 0],
              [0, 10]])
# 过程噪声协方差矩阵
Q = np.array([[0.05, 0.02, 0, 0],
              [0.02, 0.03, 0, 0], 
              [0, 0, 0.9, 0], 
              [0, 0, 0, 0.8]])
# 测量噪声协方差矩阵
R = np.array([[5.0, 0.3],
              [0.3, 7.0]])


# 构造理论值，方便评估精度，实际无法获得
t = np.linspace(0, 10, 400)
x = t
y = 10 * np.sin(t)
ideal_values = np.column_stack((x, y))

ideal_vxs = []
ideal_vys = []

ideal_vxs.append(0)
ideal_vys.append(0)

for i in range(1, len(ideal_values)):
    vx = np.abs(ideal_values[i][0] - ideal_values[i-1][0])
    vy = np.abs(ideal_values[i][1] - ideal_values[i-1][1])
    ideal_vxs.append(vx)
    ideal_vys.append(vy)


# 系列观测与控制输入
z = ideal_values + np.random.normal(0, 0.1, ideal_values.shape)
u = np.zeros_like(ideal_values)
measure_vxs = np.abs(ideal_vxs + np.random.normal(0, 0.1, len(ideal_vxs)))
measure_vys = np.abs(ideal_vys + np.random.normal(0, 0.1, len(ideal_vys)))

# 给状态赋初值
prev_state = np.array([1, 1, 1, 1])
prev_error = np.array([[100, 0, 0, 0],
                       [0, 100, 0, 0], 
                       [0, 0, 100, 0], 
                       [0, 0, 0, 100]])

# 用于存放估计的状态
predict_states = []
predict_xs = []
predict_ys = []

# 开始卡尔曼滤波
for i in range(len(z)):
    # 预测 - 状态
    predict_state = F @ prev_state + B @ u[i]
    # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (z[i] - H @ predict_state)
    # 更新 - 误差
    update_error = (np.eye(4) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state)
    predict_xs.append(update_state[0])
    predict_ys.append(update_state[1])


# 精度评价
predict_pos_errors = []
predict_vel_errors = []

measure_pos_errors = []
measure_vel_errors = []

for i in range(len(predict_states)):
    cur_state = predict_states[i]
    
    dx = np.abs(cur_state[0] - ideal_values[i][0])
    dy = np.abs(cur_state[1] - ideal_values[i][1])
    predict_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(np.abs(cur_state[2]) - ideal_vxs[i])
    dvy = np.abs(np.abs(cur_state[3]) - ideal_vys[i])
    predict_vel_errors.append(np.sqrt(dvx**2 + dvy**2))
    
    dx = np.abs(z[i][0] - ideal_values[i][0])
    dy = np.abs(z[i][1] - ideal_values[i][1])
    measure_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(measure_vxs[i] - ideal_vxs[i])
    dvy = np.abs(measure_vxs[i] - ideal_vys[i])
    measure_vel_errors.append(np.sqrt(dvx**2 + dvy**2))

print("mean measure pos:", round(np.mean(measure_pos_errors), 5))
print("mean predict pos:", round(np.mean(predict_pos_errors), 5))

print("mean measure vel:", round(np.mean(measure_vel_errors), 5))
print("mean predict vel:", round(np.mean(predict_vel_errors), 5))

# 可视化
plt.figure(figsize=(10, 6))
plt.plot(ideal_values[:,0], ideal_values[:,1], 'g-', label='ground truth')
plt.plot(z[:,0], z[:,1], 'r-', label='GPS Measure')
plt.plot(predict_xs, predict_ys, 'b-', label='Predict')
plt.legend()
plt.title('GPS Traj.')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.grid(True)
plt.show()

import numpy as np

# 给定参数
# 观测矩阵
H = np.array([[0.5], 
              [0.5], 
              [0.5], 
              [0.5]])
# 状态转移矩阵
F = np.array([[1.0]])
# 控制输入矩阵
B = np.array([[100.0]])
# 过程噪声协方差矩阵
Q = np.array([[0.1]])
# 测量噪声协方差矩阵
R = np.array([[0.5, 0, 0, 0], 
              [0, 0.6, 0, 0],
              [0, 0, 0.4, 0], 
              [0, 0, 0, 0.3]])


# 指定理论值，方便评估精度，实际无法获得
ideal_value = np.zeros(12) + 200

# 系列观测与控制输入
z = np.array([[102, 103, 101, 100.5],
              [104, 110, 103, 102],
              [98, 109, 104, 107],
              [99.7, 95, 99, 104],
              [102, 110, 98, 102],
              [101.5, 103, 105, 96],
              [100.4, 102, 103, 103],
              [101.6, 110, 104, 107],
              [103.8, 94, 98, 99],
              [97, 102, 99, 101],
              [102.1, 102, 99, 104],
              [99.3, 108, 102, 103]])

u = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

# 给状态赋初值
prev_state = np.array([[204]])
prev_error = np.array([[1.0]])

# 用于存放估计的状态
predict_states = []

measure_states = []

# 开始卡尔曼滤波
for i in range(len(z)):
    # # 预测 - 状态
    predict_state = F @ prev_state + B @ np.array([[u[i]]])
    # # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (np.array([z[i]]).T - H @ predict_state)
    
    # 更新 - 误差
    update_error = (np.eye(1) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state[0][0])
    measure_states.append(np.mean(z[i] * 2))

# 精度评价
measure_errors = np.abs(measure_states - ideal_value)
predict_errors = np.abs(predict_states - ideal_value)

for i in range(len(predict_errors)):
    print(i+1, "/", len(predict_errors), "predict error:", round(predict_errors[i], 5), "measure error:", round(measure_errors[i], 5))

print("mean measure", round(np.mean(measure_errors), 5))
print("mean predict", round(np.mean(predict_errors), 5))

import numpy as np
from matplotlib import pyplot as plt

# 给定参数
# 两个状态间的时间间隔
delta_t = 1

# 观测矩阵
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0],
              [1, 0, 0, 0],
              [0, 1, 0, 0]])
# 状态转移矩阵
F = np.array([[1, 0, delta_t, 0],
              [0, 1, 0, delta_t],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])
# 控制输入矩阵
B = np.array([[0, 0],
              [0, 0],
              [10, 0],
              [0, 10]])
# 过程噪声协方差矩阵
Q = np.array([[0.05, 0.02, 0, 0],
              [0.02, 0.03, 0, 0], 
              [0, 0, 0.9, 0], 
              [0, 0, 0, 0.8]])
# 测量噪声协方差矩阵
R = np.array([[5, 0.3, 0, 0],
              [0.3, 7, 0, 0], 
              [0, 0, 10, 1], 
              [0, 0, 1, 9]])


# 构造理论值，方便评估精度，实际无法获得
t = np.linspace(0, 2*np.pi, 100)
x = 10 * np.cos(t)
y = 10 * np.sin(t)
ideal_values = np.vstack([x, y])

ideal_vxs = []
ideal_vys = []

ideal_vxs.append(0)
ideal_vys.append(0)

for i in range(1, len(ideal_values[0])):
    vx = np.abs(ideal_values[0][i] - ideal_values[0][i-1])
    vy = np.abs(ideal_values[1][i] - ideal_values[1][i-1])
    ideal_vxs.append(vx)
    ideal_vys.append(vy)

# 系列观测与控制输入
z_lidar = ideal_values + np.random.normal(0, 0.5, ideal_values.shape)
z_lidar = z_lidar.T
z_gps = ideal_values + np.random.normal(0, 1, ideal_values.shape)
z_gps = z_gps.T
u = np.zeros_like(ideal_values)
u = u.T
measure_vxs = np.abs(ideal_vxs + np.random.normal(0, 0.1, len(ideal_vxs)))
measure_vys = np.abs(ideal_vys + np.random.normal(0, 0.1, len(ideal_vys)))

# 给状态赋初值
prev_state = np.array([0.2, 0.3, 1, 1])
prev_error = np.array([[100, 0, 0, 0],
                       [0, 100, 0, 0], 
                       [0, 0, 100, 0], 
                       [0, 0, 0, 100]])

# 用于存放估计的状态
predict_states = []
predict_xs = []
predict_ys = []

# 开始卡尔曼滤波
for i in range(len(z_lidar)):
    # 预测 - 状态
    predict_state = F @ prev_state + B @ u[i].T
    # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (np.concatenate([z_lidar[i], z_gps[i]]) - H @ predict_state)
    # 更新 - 误差
    update_error = (np.eye(4) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state)
    predict_xs.append(update_state[0])
    predict_ys.append(update_state[1])


# 精度评价
predict_pos_errors = []
predict_vel_errors = []

measure_pos_errors = []
measure_vel_errors = []

for i in range(len(predict_states)):
    cur_state = predict_states[i]
    
    dx = np.abs(cur_state[0] - ideal_values[0][i])
    dy = np.abs(cur_state[1] - ideal_values[1][i])
    predict_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(np.abs(cur_state[2]) - ideal_vxs[i])
    dvy = np.abs(np.abs(cur_state[3]) - ideal_vys[i])
    predict_vel_errors.append(np.sqrt(dvx**2 + dvy**2))
    
    dx = np.abs((z_lidar[i][0]+z_gps[i][0])/2 - ideal_values[0][i])
    dy = np.abs((z_lidar[i][1]+z_gps[i][1])/2 - ideal_values[1][i])
    measure_pos_errors.append(np.sqrt(dx**2 + dy**2))
    
    dvx = np.abs(measure_vxs[i] - ideal_vxs[i])
    dvy = np.abs(measure_vxs[i] - ideal_vys[i])
    measure_vel_errors.append(np.sqrt(dvx**2 + dvy**2))

print("mean measure pos:", round(np.mean(measure_pos_errors), 5))
print("mean predict pos:", round(np.mean(predict_pos_errors), 5))

print("mean measure vel:", round(np.mean(measure_vel_errors), 5))
print("mean predict vel:", round(np.mean(predict_vel_errors), 5))

# 可视化
plt.figure(figsize=(10, 6))
plt.plot(ideal_values[0,:], ideal_values[1,:], 'g-', label='Ground Truth')
plt.plot(z_gps[:,0], z_gps[:,1], 'r-', label='GPS Measure')
plt.plot(z_lidar[:,0], z_lidar[:,1], 'y-', label='LiDAR Measure')
plt.plot(predict_xs, predict_ys, 'b-', label='Predict')
plt.legend()
plt.title('GPS Traj.')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.grid(True)
plt.show()

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter:
    def __init__(self, q, r):
        self.q = q  # 过程噪声方差
        self.r = r  # 测量噪声方差
        self.x = 0  # 初始状态估计
        self.p = 1  # 初始估计误差协方差

    def update(self, z):
        # 预测步骤
        x_pred = self.x
        p_pred = self.p + self.q

        # 更新步骤
        k = p_pred / (p_pred + self.r)
        self.x = x_pred + k * (z - x_pred)
        self.p = (1 - k) * p_pred
        return self.x

def simulate_data(n=100):
    # 生成真实轨迹（正弦波+线性趋势）
    t = np.linspace(0, 10, n)
    truth = 2 * np.sin(t) + 0.1 * t
    # 添加测量噪声
    noise = np.random.normal(0, 1, n)
    return t, truth, truth + noise

def main():
    t, truth, measurements = simulate_data()
    
    # 测试不同Q/R组合
    params = [
        {'q': 0.01, 'r': 1, 'label': 'Q=0.01, R=1 (Conservative)'},
        {'q': 0.1, 'r': 1, 'label': 'Q=0.1, R=1 (Balenced)'},
        {'q': 1, 'r': 1, 'label': 'Q=1, R=1 (Aggressive)'},
        {'q': 0.1, 'r': 0.01, 'label': 'Q=0.1, R=0.01 (Trust Measurement)'}
    ]

    plt.figure(figsize=(12, 6))
    plt.plot(t, truth, 'k-', linewidth=2, label='GT')
    plt.plot(t, measurements, 'r.', alpha=0.3, label='Measurement')

    for param in params:
        kf = KalmanFilter(param['q'], param['r'])
        estimates = [kf.update(z) for z in measurements]
        plt.plot(t, estimates, '--', linewidth=1.5, label=param['label'])

    plt.title('Q/R Test')
    plt.xlabel('Time Step')
    plt.ylabel('State')
    plt.legend()
    plt.tight_layout()
    plt.grid(True)
    plt.show()

if __name__ == '__main__':
    main()

import numpy as np
from matplotlib import pyplot as plt
import math

# 给定参数
# 两个状态间的时间间隔
delta_t = 0.1

# 观测矩阵
def getJacobianH(state, landmark_pos):
    # 获取从状态向量中获取需要的变量进行计算
    cur_x = state[0]
    cur_y = state[1]
    x_l = landmark_pos[0]
    y_l = landmark_pos[1]
    
    dx = x_l - cur_x
    dy = y_l - cur_y
    d = math.sqrt(dx*dx + dy*dy)
    
    H = np.array([[-dx/d, -dy/d, 0],
                  [dy/d, -dx/d, -1]])
    return H

# 状态转移矩阵
def getJacobianF(state, u, dt):
    # 获取从状态向量和控制向量中获取需要的变量进行计算
    theta_rad = state[2]
    vel = u[0]
    F = np.array([[1, 0, -math.sin(theta_rad)*vel*dt],
                  [0, 1, math.cos(theta_rad)*vel*dt],
                  [0, 0, 1]])
    return F

# 过程噪声协方差矩阵
Q = np.array([[0.1, 0, 0],
              [0, 0.1, 0], 
              [0, 0, 0.02]])

# 测量噪声协方差矩阵
R = np.array([[0.5, 0],
              [0, 0.17]])

# 给定路标位置
landmark_pos = np.array([5, 3])

# 控制输入，给定不同阶段的外部控制变量
controls = []
for i in range(100):
    # 不同阶段速度不同
    if i < 30:
        vel = 1.0
        omega = 0.2
    elif 30 <= i < 70:
        vel = 0.8
        omega = -0.3
    else:
        vel = 1.2
        omega = 0.1
    
    controls.append([vel, omega])


# 构造理论值，方便评估精度，实际无法获得
# 理想状态，包括 位置x、位置y、航向角theta
ideal_states = []
ideal_states.append([0, 0, 0])

for i in range(1, 100):
    x = ideal_states[i-1][0] + controls[i][0] * math.cos(ideal_states[i-1][2]) * delta_t
    y = ideal_states[i-1][1] + controls[i][0] * math.sin(ideal_states[i-1][2]) * delta_t
    theta = ideal_states[i-1][2] + controls[i][1] * delta_t
    ideal_states.append([x, y, theta])

# 根据理论状态值，构造理论观测值和噪声观测值
ideal_measurements = []
noisy_measurements = []
for i in range(len(ideal_states)):
    cur_x = ideal_states[i][0]
    cur_y = ideal_states[i][1]
    cur_theta = ideal_states[i][2]
    
    ideal_d = math.sqrt((landmark_pos[0] - cur_x)**2 + (landmark_pos[1] - cur_y)**2)
    ideal_a = math.atan2(landmark_pos[1] - cur_y, landmark_pos[0] - cur_x) - cur_theta
    
    noisy_d = ideal_d + np.random.normal(0, 0.1)
    noisy_a = ideal_a + np.random.normal(0, 0.1)
    
    ideal_measurements.append([ideal_d, ideal_a])
    noisy_measurements.append([noisy_d, noisy_a])

# 系列观测和外部控制
z = noisy_measurements
u = controls

# 给状态赋初值
prev_state = np.array([0, 0, 0])
prev_error = np.array([[1, 0, 0],
                       [0, 1, 0], 
                       [0, 0, 1]])

# 用于存放估计的状态
predict_states = []
predict_xs = []
predict_ys = []

# 开始卡尔曼滤波
for i in range(len(z)):
    # 预测 - 状态
    # predict_state = F @ prev_state + B @ u[i]
    x_t = prev_state[0] + u[i][0] * math.cos(prev_state[2]) * delta_t
    y_t = prev_state[1] + u[i][0] * math.sin(prev_state[2]) * delta_t
    theta_t = prev_state[2] + u[i][1] * delta_t
    predict_state = np.array([x_t, y_t, theta_t])
    
    # 预测 - 计算雅可比矩阵F
    # 在t-1时刻泰勒展开，所以传入上次的状态
    F = getJacobianF(prev_state, u[i], delta_t)
    
    # 预测 - 误差
    predict_error = F @ prev_error @ F.T + Q
    
    # 更新 - 计算雅可比矩阵H
    # 在t时刻泰勒展开，所以传入当前状态
    H = getJacobianH(predict_state, landmark_pos)
    
    # 更新 - 卡尔曼增益
    kalman_gain  = predict_error @ H.T @ np.linalg.inv(H @ predict_error @ H.T + R)
    
    # 更新 - 状态
    update_state = predict_state + kalman_gain @ (z[i] - H @ predict_state)
    
    # 更新 - 误差
    update_error = (np.eye(3) - kalman_gain @ H) @ predict_error
    
    # 变量赋值开启下次迭代
    prev_state = update_state
    prev_error = update_error
    
    # 将预测状态保存起来
    predict_states.append(update_state)
    predict_xs.append(update_state[0])
    predict_ys.append(update_state[1])

# 可视化
plt.figure(figsize=(10, 6))
plt.plot(predict_xs, predict_ys, 'o-', label='Predict')
plt.scatter(*landmark_pos, c='red', marker='*', s=200, label='Landmark')
plt.legend()
plt.title('Traj.')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.grid(True)
plt.show()

import numpy as np
from scipy.linalg import expm

# 系统参数初始化
dt = 0.01  # 采样时间间隔(秒)
state_dim = 9  # 状态维度 [位置x,y,z, 速度vx,vy,vz, 加速度bias ax_b,ay_b,az_b]
state = np.zeros(state_dim)  # 初始状态
P = np.eye(state_dim) * 10  # 初始协方差矩阵

# IMU噪声参数
gyro_noise = 0.001  # 陀螺仪噪声(rad/s)
accel_noise = 0.01  # 加速度计噪声(m/s²)
bias_noise = 1e-5    # 加速度计偏置噪声

# GPS噪声参数
gps_pos_noise = 0.5  # 位置测量噪声(m)

# 过程噪声矩阵
Q = np.diag([0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.01, 0.01, 0.01])

# 观测噪声矩阵
R_gps = np.diag([gps_pos_noise, gps_pos_noise, gps_pos_noise])

def skew(v):
    """计算向量v的斜对称矩阵"""
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def rotation_matrix(phi, theta, psi):
    """计算旋转矩阵(滚转phi, 俯仰theta, 偏航psi)"""
    R_x = np.array([[1, 0, 0],
                    [0, np.cos(phi), -np.sin(phi)],
                    [0, np.sin(phi), np.cos(phi)]])
    
    R_y = np.array([[np.cos(theta), 0, np.sin(theta)],
                    [0, 1, 0],
                    [-np.sin(theta), 0, np.cos(theta)]])
    
    R_z = np.array([[np.cos(psi), -np.sin(psi), 0],
                    [np.sin(psi), np.cos(psi), 0],
                    [0, 0, 1]])
    
    return R_z @ R_y @ R_x

def predict(imu_accel, imu_gyro):
    """EKF预测步骤（处理IMU数据）"""
    global state, P
    
    # 解包状态变量
    x, y, z, vx, vy, vz, ax_b, ay_b, az_b = state
    
    # 估计姿态（简化的四元数更新）
    phi, theta, psi = 0, 0, 0  # 实际应用中需通过陀螺仪积分计算
    R = rotation_matrix(phi, theta, psi)
    
    # 校正加速度计偏置
    corrected_accel = imu_accel - [ax_b, ay_b, az_b]
    
    # 将加速度转换到全局坐标系
    global_accel = R @ corrected_accel + np.array([0, 0, -9.81])
    
    # 非线性状态转移函数
    def f(x):
        px, py, pz, vx, vy, vz, ax_b, ay_b, az_b = x
        return np.array([
            px + vx * dt + 0.5 * global_accel[0] * dt**2,
            py + vy * dt + 0.5 * global_accel[1] * dt**2,
            pz + vz * dt + 0.5 * global_accel[2] * dt**2,
            vx + global_accel[0] * dt,
            vy + global_accel[1] * dt,
            vz + global_accel[2] * dt,
            ax_b,
            ay_b,
            az_b
        ])
    
    # 计算雅可比矩阵
    F = np.eye(state_dim)
    F[0, 3] = dt  # ∂x/∂vx
    F[1, 4] = dt  # ∂y/∂vy
    F[2, 5] = dt  # ∂z/∂vz
    F[3, 6:9] = -R[0] * dt  # ∂vx/∂bias
    F[4, 6:9] = -R[1] * dt  # ∂vy/∂bias
    F[5, 6:9] = -R[2] * dt  # ∂vz/∂bias
    
    # 状态预测
    state = f(state)
    
    # 协方差预测
    P = F @ P @ F.T + Q

def update_gps(gps_pos):
    """EKF更新步骤（处理GPS数据）"""
    global state, P
    
    # 观测矩阵（GPS直接观测位置）
    H = np.array([
        [1, 0, 0, 0, 0, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 1, 0, 0, 0, 0, 0, 0]
    ])
    
    # 卡尔曼增益计算
    S = H @ P @ H.T + R_gps
    K = P @ H.T @ np.linalg.inv(S)
    
    # 状态更新
    innovation = gps_pos - state[:3]
    state += K @ innovation
    
    # 协方差更新
    P = (np.eye(state_dim) - K @ H) @ P
    
    # 姿态归一化（实际应用中需处理四元数）

# 主函数
def main():
    # 模拟数据
    np.random.seed(42)
    
    # 真实轨迹（圆周运动）
    t = 0
    radius = 50
    angular_vel = 0.1
    
    for step in range(1000):
        t = step * dt
        
        # 生成真实状态
        true_pos = np.array([
            radius * np.cos(angular_vel * t),
            radius * np.sin(angular_vel * t),
            10.0
        ])
        true_vel = np.array([
            -radius * angular_vel * np.sin(angular_vel * t),
            radius * angular_vel * np.cos(angular_vel * t),
            0
        ])
        
        # 生成IMU测量值（含噪声）
        imu_accel = true_vel * angular_vel + np.random.normal(0, accel_noise, 3)
        imu_gyro = np.array([0, 0, angular_vel]) + np.random.normal(0, gyro_noise, 3)
        
        # 生成GPS测量值（含噪声）
        if step % 10 == 0:  # GPS频率10Hz
            gps_data = true_pos + np.random.normal(0, gps_pos_noise, 3)
        
        # EKF处理
        predict(imu_accel, imu_gyro)
        if step % 10 == 0:
            update_gps(gps_data)
        
        # 每100步输出结果
        if step % 100 == 0:
            print(f"Step {step}: Pos={state[:3]}, Vel={state[3:6]}")

if __name__ == "__main__":
    main()