자파리 취미생활 관련 제품 사용기나 팁, 활용법 등을 소개하는 블로그입니다. 드론, RC, 드로잉, 자전거, 여행 등을 주제로 합니다.

아래는 챗GPT를 이용하여 코드를 짜 본 예시입니다.

상보 필터와 칼만필터를 적용하도록 지시해 보았습니다.

상보 필터와 칼만필터는 이 분야를 아주 깊게 파면서 공부하는 사람이 아니라면 이해하기가 매우 어렵습니다.

그런 이유로 복잡해서 전부 이해되지는 않지만 일견하기에는 작동할 것 같이 보입니다.

아래 값들은 목적, 결선 및 작동하는 모습에 따라 일부 변화를 주어야 할 것 같습니다.

servoPin 값

servooffset 값

pwmPin 값

Serial.begin 값 (baudrate)

dt 값

#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>

MPU6050 mpu;
Servo servoMotor;

int servoPin = 9; // 서보모터 연결 핀 번호
int servoOffset = 90; // 초기 각도 오프셋

int gyroX, gyroY, gyroZ;
float pitch, roll;

const int pwmPin = 8; // PWM 신호를 입력받을 핀 번호

// 칼만 필터 변수
float qAngle = 0.001; // 프로세스 노이즈 공분산
float qGyro = 0.003; // 자이로스코프 노이즈 공분산
float rAngle = 0.03; // 측정 노이즈 공분산
float angle = 0; // 필터링된 각도
float bias = 0; // 각도 편향
float rate; // 자이로스코프 각속도

void setup() {
  Wire.begin();
  Serial.begin(9600); // 시리얼 통신의 baud rate를 9600으로 설정
  
  mpu.initialize();
  
  servoMotor.attach(servoPin);
  servoMotor.write(servoOffset); // 초기 위치로 이동
  delay(500);
  
  pinMode(pwmPin, INPUT); // PWM 신호 입력 설정
  
  // 자이로스코프 보정
  calculateOffsets();
}

void loop() {
  mpu.getMotion6(&gyroX, &gyroY, &gyroZ, NULL, NULL, NULL);
  
  // 자이로스코프 데이터를 각도로 변환
  roll = gyroX * 0.0000611;
  pitch = gyroY * 0.0000611;
  
  // 상보 필터 적용
  pitch = alpha * (pitch + gyroY * 0.0000611) + (1 - alpha) * pitch;
  
  // PWM 신호 읽기
  int pwmValue = pulseIn(pwmPin, HIGH, 20000); // 20ms 타임아웃
  
  // 칼만 필터 적용, dt값을 줄이면 변화가 느려지며 크게하면 변화가 빠른대신 값이 튄다
  float dt = 0.02; // 시간 간격 (초 단위)
  kalmanFilter(dt);
  
  // 서보모터 각도 계산
  int servoAngle = map(angle, -90, 90, 0, 180); // 각도 범위 조정 가능
  
  // 서보모터 제어
  servoMotor.write(servoAngle);
  
  delay(20);
}

void calculateOffsets() {
  int numSamples = 100;
  int xOffset = 0;
  int yOffset = 0;
  
  for (int i = 0; i < numSamples; i++) {
    mpu.getMotion6(&gyroX, &gyroY, &gyroZ, NULL, NULL, NULL);
    xOffset += gyroX;
    yOffset += gyroY;
    delay(10);
  }
  
  xOffset /= numSamples;
  yOffset /= numSamples;
  
  mpu.setXGyroOffset(-xOffset);
  mpu.setYGyroOffset(-yOffset);
}

void kalmanFilter(float dt) {
  // 예측 단계
  angle += (rate - bias) * dt;
  float pMinus = pAngle + dt * (dt * pGyro - pAngle - pGyro + qAngle);
  float k = pMinus / (pMinus + rAngle);

  // 갱신 단계
  angle += k * (pitch - angle);
  bias += k * (pitch - angle);

  pAngle = (1 - k) * pMinus;
  pGyro += qGyro;
}