Вам нужно расширить вашу модель и добавить состояния для скорости (и если вы хотите для ускорения). Фильтр оценит новые состояния на основе положения и использует их для прогнозирования положения, даже если у вас нет измерений положения.
Ваши матрицы будут выглядеть примерно так:
Матрица шумов процесса
Q = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc
Матрица измерения шума остается прежней
Ковариационная матрица
P = np.diag([0.001, 0.001, 0.1, 0.1, 0.1, 0.1]) #enter correct numbers for vel and acc
Матрица наблюдения
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]])
Матрица перехода
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]])
Государство
Посмотрите на мой старый пост с очень похожей проблемой. В этом случае были только измерения для ускорения и оценки местоположения и скорости фильтра, а также.
Использование PyKalman на необработанных данных ускорения для расчета позиции
В следующем посте нужно было также предсказать позицию. Модель состояла только из двух положений и двух скоростей. Вы можете найти матрицы в коде Python там.
Фильтр Калмана с различными временными шагами
UPDATE
Вот мой пример Matlab, чтобы показать вам оценку состояния для скорости и ускорения только из измерений положения:
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
Моделируемый сигнал положения исчезает между 40 и 58 секундами, но оценка продолжается с помощью расчетной скорости и ускорения.
Как видите, положение можно оценить даже без обновления датчика