Unscented Kalman Filtering to find displacement

Projects Programming
1

Hello, I am a final year student studying mecnhanical engineering and I am currently doing a project for a subject that I am taking. The project that I am doing is a mobile robot which is using an ESP32-WROOM-32, a MPU6050, and an ultrasonic sensor. So my problem is I wanted to find displacement using the accelerometer of the MPU6050 and I have already implemented an UKF to the accelerometer readings. I have made the code but somehow the readings that I am getting is not the displacement but whenever I tilt the robot up the readings increases and when I tilt the robot straight up the readings go up to 1.09 and then if I continue to flip it on its back the readings decreases again. Is there anything wrong with my code?

Here is my code that I am using and I am omitting the my wifi name and password and also I am omitting the urls for the sensors and also the MAC address for the ESP32 because I am using 2 mobile robots communicating between each other

#include <Wire.h>
#include <MPU6050_tockn.h>
//#include <TinyGPSPlus.h>
#include <Kalman.h>
#include <Ultrasonic.h>
//#include <esp_now.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <HTTPClient.h>

MPU6050 mpu(Wire);
//TinyGPSPlus gps;
//HardwareSerial SerialGPS(2);
WiFiClient client;
WiFiServer server(80);

// Constants for the low-pass filter
const float alphaLPF = 0.2;  // Filter coefficient (0 < alpha < 1)

// Variables for the low-pass filter
float filteredAccelerationY = 0.0;

// Constants for Kalman filter
const float dt = 0.01;        // Time step (in seconds)
const float varAcc = 10.0;     // Variance of accelerometer measurements (value usually 0.01 to 0.1)
const float varProcess = 0.01; // Variance of process noise (value usually 1.0 to 10.0)

// Variables for Kalman filter
float positionY = 0.0;
float velocityY = 0.0;
float covPositionY = 10000.0; //Value usually 100.0 to 1000.0
float covVelocityY = 10000.0; //Value usually 100.0 to 1000.0

/*
//Variables for GPS
double latitude, longitude, DataLat5, DataLat6;
String loadLat, loadlong;
String lat_str, lng_str;
String DataLat1, DataLat2, DataLat3, DataLat4;
*/

// Motor A
int enA = 13;
int in1 = 12;
int in2 = 14;

// Motor B
int enB =19;
int in3 = 15;
int in4 = 18;

// Ultrasound sensor pins
int trigPin = 2;
int echoPin = 4;

// Threshold distance for wall detection
int wallDistanceThreshold = 30; // Adjust as needed

// Create an Ultrasonic object
Ultrasonic ultrasonic(trigPin, echoPin);

const char* ssid = "xxxxx"; //gantikan dengan nama WiFi
const char* password = "xxxxx";       //gantikan dengan password WiFi

//URL link to Post Ultrasonic
//String serverNameUltrasonic ="";
//String serverNameGPS1 ="";
//String serverNameGPS2 ="";
//String serverNamePosX ="";
String serverNamePosY ="";
//String serverNameYaw ="";
unsigned long lastTime = 0;

//Set timer to 5 seconds (5000)
unsigned long timerDelay = 5000;

/*
// REPLACE WITH THE RECEIVER'S MAC Address
uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

// Structure example to send data
typedef struct struct_message {
  int id; // must be unique for each sender board
  float positionY;
} struct_message;

// Create a struct_message called myData
struct_message myData;

// Create peer interface
esp_now_peer_info_t peerInfo;

// Helper function to serialize a float value to a byte array
void serializeFloat(float value, uint8_t* buffer) {
  memcpy(buffer, &value, sizeof(float));
}
*/

void setup() {
  Wire.begin();
  Serial.begin(115200);

  // Initialize MPU6050
  mpu.begin();
  mpu.calcGyroOffsets();

  //tetapkan paparan series
  Serial.begin(115200);
  WiFi.begin(ssid, password);
  while(WiFi.status()!=WL_CONNECTED)
  {
    delay(1000);
    Serial.println("Connecting to WiFi");
  }

  Serial.println("Connected to WiFi network");
  Serial.println("IP address");
  Serial.println(WiFi.localIP());
  //SerialGPS.begin(9600, SERIAL_8N1, 16, 17);

  //tetapkan peranti sebagai stesen WiFi
  WiFi.mode(WIFI_STA);

  /*
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  
  // Register peer
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;

  // Add peer        
  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Failed to add peer");
    return;
  }
  */
}

void loop() {
  // Update MPU6050 readings
  mpu.update();
  moveDetect();

  // Get accelerometer and gyroscope data
  float AccelerationY = mpu.getAccY();
  
  // Low-Pass filter for MPU6050 acceleration
  filteredAccelerationY = alphaLPF * AccelerationY + (1 - alphaLPF) * filteredAccelerationY; 
  
  // Perform Unscented Kalman filter update step for accelerometer data
  ukfUpdate(filteredAccelerationY);

  // Print the estimated position to the Serial Monitor
  Serial.print("Estimated Position (Y): ");
  Serial.println(positionY, 2);

  /*
  // Set values to send
  myData.id = 1;
  myData.positionY = positionY;

  // Send message via ESP-NOW
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData));
   
  if (result == ESP_OK) {
    Serial.println("Sent with success");
  }
  else {
    Serial.println("Error sending the data");
  }
  */

  delay(100);
}

void moveDetect()
{
  // Read the distance from the ultrasound sensor
  float distance = ultrasonic.read();

  // Check if the distance is below the threshold
  if (distance < wallDistanceThreshold) {
    // Stop the motor
    analogWrite(enA, 0);
    analogWrite(enB, 0);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
    // Optional: Add any other actions you want to perform when the wall is detected
  } else {
    // Move the motor forward (example)
    analogWrite(enA, 150);
    analogWrite(enB, 153);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
  }

  delay(10); // Adjust delay based on the desired sampling rate
}

void ukfUpdate(float measurementY) {
  // Predict step
  float predictedPositionY = positionY + velocityY * dt;
  float predictedCovPositionY = covPositionY + covVelocityY * dt + varProcess;

  // Generate sigma points
  float sigmaPoint1Y = predictedPositionY + sqrt(predictedCovPositionY);
  float sigmaPoint2Y = predictedPositionY - sqrt(predictedCovPositionY);

  // Apply process model to sigma points
  float propagatedSigmaPoint1Y= sigmaPoint1Y + velocityY * dt;
  float propagatedSigmaPoint2Y = sigmaPoint2Y + velocityY * dt;

  // Calculate predicted measurement mean and covariance
  float predictedMeanY = (propagatedSigmaPoint1Y + propagatedSigmaPoint2Y) / 2.0;
  float predictedCovMeasurementY = pow(propagatedSigmaPoint1Y - predictedMeanY, 2) + varAcc;

  // Calculate Kalman gain
  float kalmanGainY = predictedCovPositionY / (predictedCovPositionY + predictedCovMeasurementY);

  // Update step
  positionY = predictedPositionY + kalmanGainY * (measurementY - predictedMeanY)+0.04;
  covPositionY = (1 - kalmanGainY) * predictedCovPositionY;

  // Calculate velocity based on updated position
  velocityY = (positionY - predictedPositionY) / dt;

  delay(10);  // Adjust delay based on the desired sampling rate

  //memeriksa keadaan sambungan wifi
  if(WiFi.status()==WL_CONNECTED)
  {
    HTTPClient http;
    String serverPosY = serverNamePosY + positionY;

    //nama domain untuk pautan URL atau alamat internet protokol
    http.begin(serverPosY.c_str());

    //Menghantar HTTP GET request
    int httpResponseCode = http.GET();

    //Free resources
    http.end();
  }
  else
  {
    Serial.println("WiFi Disconnected");
  }
  
}

Is there anyone who can help me?

2

I have moved your Topic to Programming Questions....Please try not to post in Uncategorised again, If you are unsure about the categories refer to this guide.

3

Is this an area of expertise for you? I ask because it's a srsly hard problem to determine displacement using an accelerometer.

It's not simple like in Psychics 1.01.

Google it.

a7

4

I also have a GPS module the GY-GPS6MV2 but will that help in finding the displacement and if so how should I tackle this problem? I am only trying to find displacement between 2 points that is moving in a straight line the issue is the motor I am using is does not have a rotary encoder that I can use to calculate the displacement.

5

Consumer grade accelerometers are too inaccurate to be useful for estimating distance or velocity.

GPS generally does not work indoors, and out of doors, with a clear view of the sky, you can expect at best +/- 2 m accuracy for each location point.

6

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.