Self Balancing Robot V1 (Prototype)

void KalmanFilter::predict(float *gyro) { 
    // A Priori State Estimate with Gyroscope Readings ( New_Angle = Old_Angle + (Angular_Velocity * dt_sec) )
    roll  += gyro[0] * RAD_TO_DEG * dt_sec;
    pitch += gyro[1] * RAD_TO_DEG * dt_sec;

    // Uncertainty Grows (To account for gyroscopic drift, inject Process Noise Q into the Covariance matrix)
    Sigma[0] += Q[0] * dt_sec;
    Sigma[3] += Q[1] * dt_sec;
}
            

void KalmanFilter::measurement_task(float *accel) {
    // Calculate True Measured Tilt with Accelerometer Readings
    m_roll  = atan2(accel[1],  sqrt(sqr(accel[0]) + sqr(accel[2]))) * RAD_TO_DEG;
    m_pitch = atan2(-accel[0], sqrt(sqr(accel[1]) + sqr(accel[2]))) * RAD_TO_DEG;
    
    // Compute Innovation Covariance S (Fuse initial uncertainty with Accelerometer measurement Noise R)
    float S0 = Sigma[0] + R[0];
    float S1 = Sigma[1];
    float S2 = Sigma[2];
    float S3 = Sigma[3] + R[1];

    // Compute Kalman Gains (($K = \Sigma * S^(-1)$))
    k_det = 1.0f / (S0 * S3 - S1 * S2);

    k_gain[0] = (Sigma[0] * S3 - Sigma[1] * S2) * k_det;
    k_gain[1] = (Sigma[1] * S0 - Sigma[0] * S1) * k_det;
    k_gain[2] = (Sigma[2] * S3 - Sigma[3] * S2) * k_det;
    k_gain[3] = (Sigma[3] * S0 - Sigma[2] * S1) * k_det;

    // Calculate the Error Between the Accelerometer Reading and the A Priori Estimate
    float r_error = m_roll - roll;
    float p_error = m_pitch - pitch;

    // Update the Roll and Pitch with Kalman Gains
    roll  += (k_gain[0] * r_error) + (k_gain[1] * p_error);
    pitch += (k_gain[2] * r_error) + (k_gain[3] * p_error);

    // Update Error Covariance Matrix (A Posteriori Estimation)
    float s0 = Sigma[0], s1 = Sigma[1], s2 = Sigma[2], s3 = Sigma[3];
    Sigma[0] = (1.0f - k_gain[0]) * s0 - k_gain[1] * s2;
    Sigma[1] = (1.0f - k_gain[0]) * s1 - k_gain[1] * s3;
    Sigma[2] = -k_gain[2] * s0 + (1.0f - k_gain[3]) * s2;
    Sigma[3] = -k_gain[2] * s1 + (1.0f - k_gain[3]) * s3;

    // Enforce Matrix Symmetry (If any rounding errors accumulate)
    float relationship_avg = (Sigma[1] + Sigma[2]) * 0.5f;
    Sigma[1] = Sigma[2] = relationship_avg;
}
            

float PID::control(float cur_angle, float max)
{
    error = setpoint - cur_angle;

    // DERIVATIVE: Compute raw rate of error change and apply an 80/20 discrete low-pass filter
    float raw_derivative = (error - previous_error) / dt;
    derivative = (0.80f * previous_derivative) + (0.2f * raw_derivative);
    
    previous_error = error;
    previous_derivative = derivative;

    // INTEGRAL: Constrained to prevent integral windup from saturating the motor capacity
    integral = constrain((integral + (error * dt)), -1.20f, 1.20f);

    // Output the PID corrected motor output
    out = (kp * error) + (ki * integral) + (kd * derivative);
    return constrain(out, -max, max); 
}