python实现卡尔曼滤波+PID模拟简单定高飞行
模拟事件
飞机从地面起飞到目标高度并停留。
只跟踪状态量——飞机高度(exp_alt),传感器只获取飞机高度值。
实现步骤
-------------------------------PID部分-----------------------------------
1、获取飞机当前时刻高度(pre_alt);
2、计算与目标高度的PID误差,即比例误差(now_err)、积分误差(sum_err)、微分误差(now_err–last_err);
3、PID根据误差输出飞机加速度(考虑实际情况,加速度acc最大不超过20m/s**2);
--------------------------卡尔曼滤波部分-----------------------------
4、飞机根据加速度进行高度预测(pre_alt);
5、给预测值加入过程噪声,用于模拟飞机所处的真实高度(true_alt);
6、根据真实高度(true_alt),通过 true_alt+噪声 的方式模拟传感器所获取的值(sensor);
7、综合预测高度(pre_alt)以及传感器的值(sensor),做出新的预测值(pre_alt);
8、返回步骤一。
注:这里仅简单采用了卡尔曼的思想,并未通过过程误差和传感器误差来确定K值,也即省略了卡尔曼中的三条公式(两条P公式,一条K公式,可自行添加)
仿真代码
import numpy as np
import matplotlib.pyplot as plt
class Altitude():
def __init__(self, exp_alt, true_alt, kp, ki, kd):
self.true_alt = true_alt
self.pre_alt = true_alt + np.random.normal(0, 0.1)
self.exp_alt = exp_alt
self.sensor = 0
self.kp = kp
self.ki = ki
self.kd = kd
self.sum_err = 0
self.now_err = 0
self.last_err = 0
self.aim_alt = 0
self.acc = 0
self.time_step = 0.1
self.vel = 0
self.K = 0.8
def PID(self):
self.last_err = self.now_err
self.now_err = self.exp_alt - self.pre_alt
self.sum_err += self.now_err
self.aim_alt = self.kp *self.now_err + self.ki *self.sum_err + self.kd *(self.now_err - self.last_err)
self.acc = 2*(self.aim_alt - self.vel *self.time_step)/(self.time_step**2)
if self.acc >= 20:
self.acc = 20
if self.acc <= -20:
self.acc = -20
return self.acc, self.aim_alt
def KF(self):
self.pre_alt = self.pre_alt + self.vel *self.time_step + 0.5*self.acc*(self.time_step**2)
self.true_alt = self.true_alt + self.vel * self.time_step + 0.5 * self.acc * (self.time_step ** 2)
self.vel = self.vel + self.acc*self.time_step
if self.vel >= 20:
self.vel = 20
if self.vel <= -20:
self.vel = -20
self.true_alt = self.true_alt + np.random.normal(0,0.1)
self.sensor = self.true_alt + np.random.normal(0,0.1)
self.pre_alt = self.pre_alt + self.K*(self.sensor - self.pre_alt)
return self.pre_alt, self.true_alt, self.vel
pre = []
true = []
vel = []
altitude = Altitude(100, 0, 0.2, 0.00005, 0.05)
for i in range(1,200):
acc, aim_alt = altitude.PID()
pre_alt, true_alt, now_vel = altitude.KF()
pre.append(pre_alt)
true.append(true_alt)
vel.append(now_vel)
time = list(range(0, 200))
plt.plot(time, pre, color='blue', label='pre')
plt.plot(time, true, color='red',label='true')
plt.show()
效果
200个时间步内飞机的高度裱花情况
100-200时间步内飞机高度变化,蓝色为预测值,红色为真实值