kalman filter and prediction

I've tweaked this code countless times, but I'm still scratching my head over why the Kalman filter predictions are way off. Any hints or insights to help me unravel this mystery would be greatly appreciated. Thanks a lot!

% Example usage
clc
clear all
close all
opposite_velocity = 5*sqrt(2);
x_initial = -5;
y_initial = 10;
time = linspace(0, 1, 200);
random_noise_x = randn(size(time));
random_noise_y = randn(size(time));

x = x_initial + time * opposite_velocity * sind(45);
y= y_initial - time * opposite_velocity * sind(45) ;
dt = 1/200;               % Replace with your actual time step
numSteps = 3;         % Number of steps to predict

[estimatedStates, ~, predictedStates, predictedVelocities] = kalman_Filter(x, y, dt, numSteps);

function [estimatedStates, covarianceMatrix, predictedStates, predictedVelocities] = kalman_Filter(x, y, dt, numSteps)
    % Initialize Kalman filter parameters
    A = [1 dt; 0 1];  % State transition matrix
    H = [1 0; 0 1];   % Measurement matrix
    Q = diag([0.9, 0.9]); % Process noise covariance
    R = diag([0.1, 0.1]); % Measurement noise covariance

    % Initialize state and covariance matrix
    initialState = [x(1); (y(2) - y(1)) / dt];
    initialCovariance = eye(2);

    currentState = initialState;
    covarianceMatrix = initialCovariance;

    % Kalman filter loop
    estimatedStates = zeros(2, length(x));
    predictedStates = zeros(2, numSteps);
    predictedVelocities = zeros(2, numSteps); % Changed to 2D array
    
    for i = 1:length(x)
        % Prediction step
        currentState = A * currentState;
        covarianceMatrix = A * covarianceMatrix * A' + Q;

        % Update step
        kalmanGain = covarianceMatrix * H' / (H * covarianceMatrix * H' + R);
        measurement = [x(i); y(i)];
        currentState = currentState + kalmanGain * (measurement - H * currentState);
        covarianceMatrix = (eye(2) - kalmanGain * H) * covarianceMatrix;

        % Store the estimated state
        estimatedStates(:, i) = currentState;

        % Update A matrix based on changing velocity
        if i < length(x)
            A(2, 2) = (y(i+1) - y(i)) / dt; % Update velocity in the A matrix
        end
    end

    % Prediction for the next few steps
    predictedStates(:, 1) = currentState;
    for j = 2:numSteps
        currentState = A * currentState;
        predictedStates(:, j) = currentState;
    end

    % Calculate velocities for estimated states
    estimatedVelocities = [diff(estimatedStates(1,:)) / dt; diff(estimatedStates(2,:)) / dt];

    % Calculate velocities for predicted states
    predictedVelocities(:, 1:end-1) = [diff(predictedStates(1,:)) / dt; diff(predictedStates(2,:)) / dt];

    % Plot the results in 3D
    figure;

    subplot(2, 1, 1);
    plot3(x, y, 1:length(x), 'o-', 'DisplayName', 'Measured Positions');
    hold on;
    plot3(estimatedStates(1, :), estimatedStates(2, :), 1:length(x), 'x-', 'DisplayName', 'Estimated States');
    plot3(predictedStates(1, :), predictedStates(2, :), (length(x)+1):(length(x)+numSteps), 's-', 'DisplayName', 'Predicted States');
    legend('Location', 'best');
    xlabel('X Position');
    ylabel('Y Position');
    zlabel('Time Step');
    title('Kalman Filter for Ship Position Estimation');
    grid on;

    subplot(2, 1, 2);
    plot3(estimatedVelocities(1,:),estimatedVelocities(2,:),1:length(x)-1,'o-', 'DisplayName', 'Estimated Velocities');
    hold on;
    plot3(predictedVelocities(1,:),predictedVelocities(2,:),length(x):(length(x)+numSteps-1), 's-', 'DisplayName', 'Predicted Velocities');
    xlabel('Velocity X Position');
    ylabel('Velocity Y Position');
    zlabel('Time Step');
    title('Velocity Magnitude over Time');
    legend('Location', 'best');
    grid on;
end

[Benjamin Thompson]

Please define x and y. Is y the velocity and x is the position? Or if x and y are both positions then your problems has four state variables, and A should be a 4x4 matrix.

If the A matrix is part of a linear time invariant model you should not change A.