kinematic_prediction.py

Last modified: 2023-07-15

View on GitHub

This file contains functions for kinematic prediction in the RoboX system.

Main Flow Diagram

Main Flow Diagram
Download Flowchart
ExplanationCode

Ensure x and y have the same shape

    assert x.shape == y.shape, "x and y arrays must have the same shape"

Ensure there are enough points for the chosen model degree

    assert x.shape[0] >= model_degree + 1, "Too few points for the chosen model degree"

Create the Vandermonde matrix

Each column represents x raised to powers from 0 to model_degree

    x_mat = np.transpose(np.array([x ** n for n in range(model_degree + 1)]))

Transpose of x_mat for matrix calculations

    x_mat_t = np.transpose(x_mat)

    if weights is None:

Standard least squares regression

Calculate coefficients using the normal equation: (X^T * X)^-1 * X^T * y

        coeffs = np.matmul(np.matmul(
            np.linalg.inv(np.matmul(x_mat_t, x_mat)),
            x_mat_t),
            y)
    else:

Weighted least squares regression

Create diagonal weight matrix

        weight_mat = np.diag(weights)

Calculate coefficients using the weighted normal equation:

(X^T * W * X)^-1 * X^T * W * y

        coeffs = np.matmul(np.matmul(np.matmul(
            np.linalg.inv(np.matmul(np.matmul(x_mat_t, weight_mat), x_mat)),
            x_mat_t),
            weight_mat),
            y)

Create array of powers of predict_x for polynomial evaluation

    predict_pt_powers = np.array([predict_x ** n for n in range(model_degree + 1)])

Evaluate the polynomial at predict_x using dot product

    return np.dot(predict_pt_powers, coeffs).item()


if __name__ == '__main__':

Test the poly_predict function

    test_x = np.array([0.0, 2.0])
    test_y = np.array([0.0, 7.5])

Perform a linear prediction (model_degree=1) at x=4.0

    prediction = poly_predict(test_x, test_y, 1, 4.0)
    print(f'Linear prediction at x=4.0: {prediction}')