mirror of
https://github.com/Ysurac/openmptcprouter-feeds.git
synced 2025-03-09 15:40:03 +00:00
34 lines
No EOL
1.4 KiB
Lua
34 lines
No EOL
1.4 KiB
Lua
local kalman = require("kalman_lib")
|
|
local matrix = require("matrix_lib")
|
|
|
|
-- Test the example of a train moving along a 1-d track
|
|
function test_train()
|
|
local k = kalman.create(2, 1)
|
|
-- The train state is a 2d vector containing position and velocity.
|
|
-- Velocity is measured in position units per timestep units.
|
|
k.state_transition = matrix.set(k.state_transition, 1.0, 1.0,
|
|
0.0, 1.0)
|
|
-- We only observe position
|
|
k.observation_model = matrix.set(k.observation_model, 1.0, 0.0)
|
|
-- The covariance matrices are blind guesses
|
|
k.process_noise_covariance = matrix.set_identity(k.process_noise_covariance)
|
|
k.observation_noise_covariance = matrix.set_identity(k.observation_noise_covariance)
|
|
-- Our knowledge of the start position is incorrect and unconfident
|
|
local deviation = 1000.0
|
|
k.state_estimate = matrix.set(k.state_estimate, 10 * deviation)
|
|
k.estimate_covariance = matrix.set_identity(k.estimate_covariance)
|
|
k.estimate_covariance = matrix.scale(k.estimate_covariance, deviation * deviation)
|
|
|
|
for i = 1, 10 do
|
|
k.observation = matrix.set(k.observation, i)
|
|
k = kalman.update(k)
|
|
end
|
|
|
|
|
|
print("estimated position: " .. tostring(k.state_estimate[1][1]))
|
|
print("estimated position: " .. tostring(k.state_estimate[2][1]))
|
|
|
|
end
|
|
|
|
test_train()
|
|
print("OK") |