You need to expand your model and add states for the velocity (and if you want for the acceleration). The filter will estimate the new states based on the position and use them to predict position even if you don't have position measurements.
Your matrices would look something like this:
Process noise matrix
Q = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc
Measurement noise matrix stays the same
Covariance matrix
P = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc
Observation matrix
H = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0, 0.0]])
Transition matrix
F = np.array([[1, 0, dt, 0, 0.5*dt**2, 0],
[0, 1, 0, dt, 0, 0.5*dt**2],
[0, 0, 1, 0, dt, 0],
[0, 0, 0, 1, 0, dt],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
State
Have a look at my old post with a very similar problem. In that case there was only a measurement for the acceleration and the filter estimated position and velocity as well.
Using PyKalman on Raw Acceleration Data to Calculate Position
In the following post one had to predict position as well. The model consisted only of two positions and two velocities. You can find the matrices in the python code there.
Kalman filter with varying timesteps
UPDATE
Here is my matlab example to show you the state estimation for velocity and acceleration only from the position measurements:
function [] = main()
[t, accX, velX, posX, accY, velY, posY, t_sens, posX_sens, posY_sens, posX_var, posY_var] = generate_signals();
n = numel(t_sens);
% state matrix
X = zeros(6,1);
% covariance matrix
P = diag([0.001, 0.001,10, 10, 2, 2]);
% system noise
Q = diag([50, 50, 5, 5, 3, 0.4]);
dt = t_sens(2) - t_sens(1);
% transition matrix
F = [1, 0, dt, 0, 0.5*dt^2, 0;
0, 1, 0, dt, 0, 0.5*dt^2;
0, 0, 1, 0, dt, 0;
0, 0, 0, 1, 0, dt;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
% observation matrix
H = [1 0 0 0 0 0;
0 1 0 0 0 0];
% measurement noise
R = diag([posX_var, posY_var]);
% kalman filter output through the whole time
X_arr = zeros(n, 6);
% fusion
for i = 1:n
y = [posX_sens(i); posY_sens(i)];
if (i == 1)
[X] = init_kalman(X, y); % initialize the state using the 1st sensor
else
if (i >= 40 && i <= 58) % missing measurements between 40 ans 58 sec
[X, P] = prediction(X, P, Q, F);
else
[X, P] = prediction(X, P, Q, F);
[X, P] = update(X, P, y, R, H);
end
end
X_arr(i, :) = X;
end
figure;
subplot(3,1,1);
plot(t, posX, 'LineWidth', 2);
hold on;
plot(t_sens, posX_sens, '.', 'MarkerSize', 18);
plot(t_sens, X_arr(:, 1), 'k.', 'MarkerSize', 14);
hold off;
grid on;
title('PositionX');
legend('Ground Truth', 'Sensor', 'Estimation');
subplot(3,1,2);
plot(t, velX, 'LineWidth', 2);
hold on;
plot(t_sens, X_arr(:, 3), 'k.', 'MarkerSize', 14);
hold off;
grid on;
title('VelocityX');
legend('Ground Truth', 'Estimation');
subplot(3,1,3);
plot(t, accX, 'LineWidth', 2);
hold on;
plot(t_sens, X_arr(:, 5), 'k.', 'MarkerSize', 14);
hold off;
grid on;
title('AccX');
legend('Ground Truth', 'Estimation');
figure;
subplot(3,1,1);
plot(t, posY, 'LineWidth', 2);
hold on;
plot(t_sens, posY_sens, '.', 'MarkerSize', 18);
plot(t_sens, X_arr(:, 2), 'k.', 'MarkerSize', 14);
hold off;
grid on;
title('PositionY');
legend('Ground Truth', 'Sensor', 'Estimation');
subplot(3,1,2);
plot(t, velY, 'LineWidth', 2);
hold on;
plot(t_sens, X_arr(:, 4), 'k.', 'MarkerSize', 14);
hold off;
grid on;
title('VelocityY');
legend('Ground Truth', 'Estimation');
subplot(3,1,3);
plot(t, accY, 'LineWidth', 2);
hold on;
plot(t_sens, X_arr(:, 6), 'k.', 'MarkerSize', 14);
hold off;
grid on;
title('AccY');
legend('Ground Truth', 'Estimation');
figure;
plot(posX, posY, 'LineWidth', 2);
hold on;
plot(posX_sens, posY_sens, '.', 'MarkerSize', 18);
plot(X_arr(:, 1), X_arr(:, 2), 'k.', 'MarkerSize', 18);
hold off;
grid on;
title('Trajectory');
legend('Ground Truth', 'Sensor', 'Estimation');
axis equal;
end
function [t, accX, velX, posX, accY, velY, posY, t_sens, posX_sens, posY_sens, posX_var, posY_var] = generate_signals()
dt = 0.01;
t=(0:dt:70)';
posX_var = 8; % m^2
posY_var = 8; % m^2
posX_noise = randn(size(t))*sqrt(posX_var);
posY_noise = randn(size(t))*sqrt(posY_var);
accX = sin(0.3*t) + 0.5*sin(0.04*t);
velX = cumsum(accX)*dt;
posX = cumsum(velX)*dt;
accY = 0.1*sin(0.5*t)+0.03*t;
velY = cumsum(accY)*dt;
posY = cumsum(velY)*dt;
t_sens = t(1:100:end);
posX_sens = posX(1:100:end) + posX_noise(1:100:end);
posY_sens = posY(1:100:end) + posY_noise(1:100:end);
end
function [X] = init_kalman(X, y)
X(1) = y(1);
X(2) = y(2);
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
The simulated position signal disappears between 40s and 58s but the estimation keeps going by means of the estimated velocity and acceleration.
As you can see the position can be estimated even without sensor update
