import serial
import numpy as np
from djitellopy import Tello
import time
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
class KalmanFilter:
def __init__(self, A, B, H, Q, R, x0, P0):
self.A = A # 시스템 모델 행렬
self.B = B # 제어 입력 행렬
self.H = H # 측정 모델 행렬
self.Q = Q # 프로세스 잡음 공분산 행렬
self.R = R # 측정 잡음 공분산 행렬
self.x = x0 # 초기 상태 추정치
self.P = P0 # 초기 오차 공분산 행렬
def predict(self, u):
# 예측 단계
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
def update(self, z):
# 업데이트 단계
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 = self.P - np.dot(np.dot(K, self.H), self.P)
# 시스템 모델 파라미터
dt = 0.1 # 시간 간격
A = np.eye(3) # 시스템 모델 행렬
B = np.eye(3) * dt # 제어 입력 행렬
H = np.eye(3) # 측정 모델 행렬
Q = np.eye(3) * 0.001 # 프로세스 잡음 공분산 행렬
R = np.eye(3) * 0.1 # 측정 잡음 공분산 행렬
x0 = np.zeros((3, 1)) # 초기 상태 추정치
P0 = np.eye(3) # 초기 오차 공분산 행렬
# 시리얼 포트 설정
ser = serial.Serial('COM5', 9600)
# 칼만 필터 객체 생성
kf = KalmanFilter(A, B, H, Q, R, x0, P0)
# 제어 입력 설정 (PID 제어 알고리즘을 사용한다고 가정)
Kp = 0.1
Ki = 0.01
Kd = 0.05
setpoint = 10.0
initial_value = 0.0
dt = 0.1
total_time = 10.0
error_integral = np.zeros((3, 1))
prev_error = np.zeros((3, 1))
# Initialize lists to store data
time_data = []
feedback_data = []
control_data = []
while True:
# 시리얼 데이터 수신
data = ser.readline().decode().strip().split(',')
print("Received data:", data) # 데이터 확인
# 데이터에서 유효한 값만 추출
valid_data = [d.split(': ')[1] for d in data if ':' in d]
print("Valid data:", valid_data) # 추출된 데이터 확인
# 필요한 만큼의 값을 부동 소수점으로 변환하여 사용
roll, pitch, yaw = map(float, valid_data[:3])
# 상태 벡터 설정
z = np.array([[roll], [pitch], [yaw]])
# 예측 단계
kf.predict(np.zeros((3, 1))) # 제어 입력은 0으로 가정
# 업데이트 단계
kf.update(z)
# 상태 추정치
estimated_roll, estimated_pitch, estimated_yaw = kf.x[0, 0], kf.x[1, 0], kf.x[2, 0]
# PID 제어 알고리즘 적용
roll_error = estimated_roll - roll
pitch_error = estimated_pitch - pitch
yaw_error = estimated_yaw - yaw
roll_command = Kp * roll_error + Ki * error_integral[0, 0] + Kd * (roll_error - prev_error[0, 0])
pitch_command = Kp * pitch_error + Ki * error_integral[1, 0] + Kd * (pitch_error - prev_error[1, 0])
yaw_command = Kp * yaw_error + Ki * error_integral[2, 0] + Kd * (yaw_error - prev_error[2, 0])
# 이전 오차 및 오차 적분 업데이트
error_integral += np.array([[roll_error], [pitch_error], [yaw_error]])
prev_error = np.array([[roll_error], [pitch_error], [yaw_error]])
# 제어 출력 수행 (실제로는 드론에 명령을 보내는 코드가 여기에 위치할 것입니다)
# 디버그 출력
print("Estimated Roll:", estimated_roll)
print("Estimated Pitch:", estimated_pitch)
print("Estimated Yaw:", estimated_yaw)
class PID:
def __init__(self, Kp, Ki, Kd, setpoint):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.setpoint = setpoint
self.prev_error = 0
self.integral = 0
def update(self, feedback_value):
error = self.setpoint - feedback_value
self.integral += error
derivative = error - self.prev_error
output = (self.Kp * error) + (self.Ki * self.integral) + (self.Kd * derivative)
self.prev_error = error
return output
# Simulation parameters
Kp = 1.0
Ki = 0.1
Kd = 0.05
setpoint = 10.0
initial_value = 0.0
dt = 0.1
total_time = 10.0
# Initialize lists to store data
time_data = []
feedback_data = []
control_data = []
# Initialize serial communication
ser = serial.Serial('COM5', 9600) # Adjust the COM port as needed
# Simulate the PID controller
pid = PID(Kp, Ki, Kd, setpoint)
time = 0.0
feedback_value = initial_value
# Create a figure and axes for plotting
fig, ax = plt.subplots(2, 1)
while time < total_time:
# Read data from serial port
line = ser.readline().decode().strip()
data = line.split(',')
if len(data) == 3:
time_data.append(float(data[0]))
feedback_data.append(float(data[1]))
control_data.append(float(data[2]))
# Update feedback value
feedback_value = feedback_data[-1]
# Update control signal
control_signal = pid.update(feedback_value)
# Plot feedback value and control signal
ax[0].plot(time_data, feedback_data, label='Feedback Value')
ax[0].axhline(y=setpoint, color='r', linestyle='r-', label='Setpoint')
ax[0].set_xlabel('Time')
ax[0].set_ylabel('Feedback Value')
ax[0].legend()
ax[1].plot(time_data, control_data, label='Control Signal', color='g')
ax[1].set_xlabel('Time')
ax[1].set_ylabel('Control Signal')
ax[1].legend()
plt.pause(0.01)
# Print current values
print(f"Time: {time_data[-1]:.2f}, Feedback: {feedback_data[-1]:.2f}, Control Signal: {control_data[-1]:.2f}")
time += dt
plt.show()
#데이터가 'edback: 1.15'와 같은 형식이라면 ':'를 기준으로 데이터를 분할하고
#필요한 부분을 추출할 수 있습니다. 이를 위해 다음과 같이 수정할 수 있습니다.