From a753da7931fcf4d3b6600da9735a050912daf8be Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 25 Jun 2024 14:16:29 +0200 Subject: [PATCH] Remove some small unstabalitiyies from GroundBallExtended --- .../ball/GroundBallExtendedKalmanFilter.cpp | 49 +++++++++---------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp index 585a78e5f..0f542810d 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp @@ -6,8 +6,8 @@ void GroundBallExtendedKalmanFilter::update(const Eigen::Vector2d &observation) y = observation - (H * X); // Variance of innovation Eigen::Matrix2d S = H * P * H.transpose() + R; - // compute kalman gain. For small matrices, Eigen's inverse function is efficient - Eigen::Matrix K = P * H.transpose() * S.inverse(); + // compute kalman gain. For small matrices, Eigen's inverse function is efficient, but this is more numerically stable + Eigen::Matrix K = P * H.transpose() * S.llt().solve(Eigen::Matrix2d::Identity()); // update state with prediction X = X + K * y; // update covariance @@ -127,37 +127,36 @@ void GroundBallExtendedKalmanFilter::predict(Time timeStamp) { dt = -vel / acceleration; } - // update the transition matrix - double velCubed = vel * vel * vel; - double vysq = currentVel.y() * currentVel.y() / velCubed; - double vxvy = -currentVel.x() * currentVel.y() / velCubed; - double vxsq = currentVel.x() * currentVel.x() / velCubed; - - // To prevent division by zero, we simply put the velocity of the ball to zero if the ball is almost lying still + // Prevent division by zero by checking if velocity is very low if (vel <= BALL_STILL_VELOCITY) { - vxsq = 0.0; - vxvy = 0.0; - vysq = 0.0; + F.setIdentity(); // Use identity matrix when ball is nearly or completely still + } else { + double velCubed = vel * vel * vel; + double vysq = currentVel.y() * currentVel.y() / velCubed; + double vxvy = -currentVel.x() * currentVel.y() / velCubed; + double vxsq = currentVel.x() * currentVel.x() / velCubed; + + F(0, 2) = dt + 0.5 * vysq * acceleration * dt * dt; + F(0, 3) = 0.5 * vxvy * acceleration * dt * dt; + F(1, 2) = 0.5 * vxvy * acceleration * dt * dt; + F(1, 3) = dt + 0.5 * vxsq * acceleration * dt * dt; + F(2, 2) = 1 + vysq * acceleration * dt; + F(2, 3) = vxvy * acceleration * dt; + F(3, 2) = vxvy * acceleration * dt; + F(3, 3) = 1 + vxsq * acceleration * dt; } - F(0, 2) = dt + 0.5 * vysq * acceleration * dt * dt; - F(0, 3) = 0.5 * vxvy * acceleration * dt * dt; - F(1, 2) = 0.5 * vxvy * acceleration * dt * dt; - F(1, 3) = dt + 0.5 * vxsq * acceleration * dt * dt; - F(2, 2) = 1 + vysq * acceleration * dt; - F(2, 3) = vxvy * acceleration * dt; - F(3, 2) = vxvy * acceleration * dt; - F(3, 3) = 1 + vxsq * acceleration * dt; - - // setting the process noise matrix (Q) + // Update the process noise matrix Q setProccessNoise(frame_dt); - // now update the state given the transition function + // Update the state X based on the transition matrix F if (vel > BALL_STILL_VELOCITY) { X.head<2>() = currentPos + currentVel * dt + 0.5 * currentVel / vel * acceleration * dt * dt; X.tail<2>() = currentVel + currentVel / vel * acceleration * dt; - } else { // Set the velocity of the ball to zero - X.tail<2>() = Eigen::Vector2d::Zero(); + } else { + X.tail<2>() = Eigen::Vector2d::Zero(); // Set velocity to zero if ball is nearly still } + + // Update the covariance matrix P P = F * P * F.transpose() + Q; } \ No newline at end of file