写个卡尔曼姿态解算的代码
时间: 2023-11-16 09:06:59 浏览: 34
下面是一个简单的卡尔曼姿态解算的代码示例:
```python
import numpy as np
class KalmanFilter:
def __init__(self, dt, q=1e-5, r=0.01):
self.dt = dt
self.q = q # 过程噪声协方差
self.r = r # 观测噪声协方差
self.A = np.array([[1, -dt], [0, 1]]) # 状态转移矩阵
self.B = np.array([[dt**2/2], [dt]]) # 输入转移矩阵
self.H = np.array([1, 0]) # 观测矩阵
self.x = np.zeros((2, 1)) # 状态向量
self.P = np.zeros((2, 2)) # 状态协方差矩阵
def update(self, z, u=0):
# 预测步骤
self.x = np.dot(self.A, self.x) + np.dot(self.B, u)
self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.q
# 更新步骤
y = z - np.dot(self.H, self.x)
S = np.dot(np.dot(self.H, self.P), self.H.T) + self.r
K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
self.x = self.x + np.dot(K, y)
self.P = np.dot((np.eye(2) - np.dot(K, self.H)), self.P)
return self.x[0]
# 示例使用
dt = 0.1 # 时间步长
q = 1e-5 # 过程噪声协方差
r = 0.01 # 观测噪声协方差
kf = KalmanFilter(dt, q, r)
# 生成模拟数据
t = np.arange(0, 10, dt)
angle_true = np.sin(t) # 真实角度
angle_noise = angle_true + np.random.normal(0, 0.1, len(t)) # 加入噪声的观测角度
# 使用卡尔曼滤波进行姿态解算
angle_filtered = []
for z in angle_noise:
angle_est = kf.update(z)
angle_filtered.append(angle_est)
# 绘制结果
import matplotlib.pyplot as plt
plt.plot(t, angle_true, label='True Angle')
plt.plot(t, angle_noise, label='Noisy Angle')
plt.plot(t, angle_filtered, label='Filtered Angle')
plt.legend()
plt.xlabel('Time')
plt.ylabel('Angle')
plt.show()
```
该代码实现了一个简单的一维卡尔曼滤波器,用于姿态解算中的角度估计。代码中包含一个KalmanFilter类,其中包含了初始化、预测和更新步骤。在示例中,我们生成了模拟数据,包括真实角度和带有噪声的观测角度。然后,使用卡尔曼滤波器对观测角度进行滤波,得到估计的角度。最后,绘制了真实角度、观测角度和滤波后的角度的对比图。