EKF Implentation

In order to allow the user to provide feedback to the system in an effort to correct for any modeling errors, we have incorporated an Extended Kalman Filter. Essentially, the EKF allows for a virtual position of the robot to be tracked within the XY-plane of the global reference frame. All cups have their own reference frame, with which a rigid body transform can be calculated between the location of a given cup and that of the virtual robot. Therefore, when a user inputs which cup they desire for the cannon to aim for, we are able to calculate the required ballistic joint angles based on the estimations of the virtual position of the robot and the ground truth location of the selected cup. Feedback occurs when the user reports where the ball actually landed. We can calculate the difference between where the user says the ball landed and where the ball should of landed, thus providing an observation residual for the EKF to use in better estimating the virtual location of the robot. The following presents the formulation our Extended Kalman Filter.

A. State Declaration

Let the state vector be S =

The state at the next time step is S’ =

This gives the following equation S‘ = f(S,W) with W =

B. Propagation

-Let S0 denote the state in the previous time step, and denote the estimate of the current state based on propagation as defined above. We allow w1 = w2 = w3 ~ N(0,σ), giving the covariance of W as a diagonal matrix with each diagonal element set to σ, a parameter which is fine tuned for our system and ensures that the variance in our estimate does not become zero after multiple observations. We now get our state and covariance prediction prior to observation as follows:

State estimate using time propagation

Covariance Estimate

Note that the above propagation step is a constant linear function.

C. Observation

-Our observation will simply be the user’s input of where the ball landed with respect to the global frame. We will use a rigid body transformation, labelled ‘T’ in Fig. 1, to relate this observation to the state of the robot. This gives rise to the following expected observation vector:

This is in the form z = h(S,V), where V = and V1 = V2 ~ N(0,σ2), representing the users feedback with tuned white noise.

The above equation is used to calculate the expected observation, while the true observation is the difference between the robots prior position and the users input. Variables Xc and Yc in the above equation represent the ground truth locations of the selected cup for the robot to attempt to shoot the ping pong ball into. Because the above equation is non-linear, we must use a First-order Taylor expansion to estimate the covariance of the observation. Thus, we need the Jacobian of h with respect to the state variables. This can be calculated as follows:

We can now formulate our Kalman update step in order to increase the accuracy of the ping pong launcher by increasing the accuracy in the estimate of the robot’s virtual location.

D. Kalman Update

-We can now update our estimates of both state and variance using the above observation in Kalman fashion as follows:

Innovation

Kalman Gain

Posteriori Estimate

Updated Covariance

-The updated state and covariance are then run through the propagation step, and a new observation step occurs after the user provides feedback. Therefore, we have defined our recursive Extended Kalman Filter used to predict the robot’s virtual location given user feedback, allowing for correction of any modeling errors within the overall system.