【飞控初体验】（二）python实现卡尔曼滤波+PID模拟简单定高飞行

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时间步内飞机高度变化，蓝色为预测值，红色为真实值