Kalman Filter for a simple 1-D problem

Hello,

I have a system that linearly changes the position (reciprocal position) and am using two sensors to meausre velocity and position of the system. The position sensor has a limited resolution, and thus I am trying to use the velocity sensor to estimate the position of the system better. In order to do this, I am using the Kalman filter, in which the estimate state is just [velocity; position] = [1 0; 0 1] [velocity; position], while the integrated velocity from the velocity sensor and the position from the position sensor are used in the measurement state (please see the code below).

close all; clear all;
    fs=1e4;
    % time step
    dt = 1/fs;

    t=0:1/fs:0.2; t=t';
    n = numel(t);

    % state matrix
    X = zeros(2,1);

    % covariance matrix
    P = zeros(2,2);

    % kalman filter output through the whole time
    X_arr = zeros(n, 2);

    % system noise
    Q = [0.04 0;
         0 1];

    % transition matrix
    F = [1 dt;
         0 1]; 

    % observation matrix 
    H = [1 0];

    % variance of signal 1 (Velocity Sensor)
    v1_var = 40; f=100;
    v1 = 5*(2*pi*f)*cos(2*pi*f*t)+v1_var*randn([length(t),1]);
    s1 = cumtrapz(t,v1);
    s1_var=v1_var/(2*pi*f);
    s1 = generate_signal(s1, s1_var);
    
%     s1 = 5*sin(2*pi*f*t)+s1_var*randn([length(t),1]);
    s1 = generate_signal(s1, s1_var);

    % variance of signal 2 (Position Sensor)
    s2_var = 0.8; f=100;
    s2 = 5*sin(2*pi*f*t)+s2_var*randn([length(t),1]);
    s2 = generate_signal(s2, s2_var);

    % fusion
    for i = 1:n
        if (i == 1)
            [X, P] = init_kalman(X, s1(i, 1)); % initialize the state using the 1st sensor
        else
            [X, P] = prediction(X, P, Q, F);

            [X, P] = update(X, P, s1(i, 1), s1(i, 2), H);
            [X, P] = update(X, P, s2(i, 1), s2(i, 2), H);
        end

        X_arr(i, :) = X;
    end

    figure; plot(t, [s1(:, 1), s2(:, 1),X_arr(:, 1)], 'LineWidth', 1); set(gca,'FontSize',12); grid on;
    legend('signal 1', 'signal 2', 'Kalman Filter');
    
function [s] = generate_signal(signal, var)
%     noise = randn(size(signal)).*sqrt(var);

    s(:, 1) = signal;
    s(:, 2) = var; 
end

function [X, P] = init_kalman(X, y)
    X(1,1) = y;
    X(2,1) = 0;

    P = [100 0;
         0   300];
end

function [X, P] = prediction(X, P, Q, F)
    X = F*X;
    P = F*P*F' + Q;
end

function [X, P] = update(X, P, y, R, H)
    Inn = y - H*X;
    S = H*P*H' + R;
    K = P*H'/S;

    X = X + K*Inn;
    P = P - K*H*P;
end

Here comes a question: if I would like to use the velocity from the velocity sensor in the estimate state and the position from the position senosr in the measurement state, how can I modify this code? Any help will be appreciated!

Best,

Max