Применение триангуляции к модели движения ТС по маршруту
Автор
Комплексирование данных от БИНС и Триангуляционных датчиков с использованием фильтра Калмана.
Реализуем модель движения по маршруту с использованием концепции фильтра Калмана.
- оценка состояния системы, значение которой хранится внутри фильтра. Начальная оценка задается извне.
Подсистема реализующая алгоритм калмана выглядит следующим образом:
In [ ]:
proj_root_folder = "$(@__DIR__)/veh_pos_test_kalman"
include("$proj_root_folder/map_params.jl")
include("$proj_root_folder/car_params.jl")
include("$proj_root_folder/sensors_params.jl");
include("$proj_root_folder/matrixes.jl");
In [ ]:
sim_model = engee.load("$(proj_root_folder)/simple_veh_model_with_positioning_kalman.engee", force=true);
engee.run(sim_model)
engee.close(sim_model, force=true);
In [ ]:
pos_vect_8 = collect(pos_by_kalman).value
pos_vect_8_calc = collect(pos_calc_by_kalman).value;
In [ ]:
plot(
map(x->x[1], pos_vect_8),
map(x->x[2], pos_vect_8),
map(x->x[3], pos_vect_8),
lw=2, color=:red, label="detectors_traj"
)
plot!(
map(x->x[1], pos_vect_8_calc),
map(x->x[2], pos_vect_8_calc),
map(x->x[3], pos_vect_8_calc),
color=:blue, label="traj",
legend=:bottomright,
legendfont = font("Times New Roman", 12),
titlefont = font("Times New Roman", 14),
title = "Траектории ЦМ при движении по KALMAN"
)
Out[0]:
In [ ]:
pos_vect_8 = collect(pos_by_kalman).value
pos_vect_8_calc = collect(pos_calc_by_kalman).value
plot(
map(x->x[1], pos_vect_8),
map(x->x[2], pos_vect_8),
lw=2, color=:red, label="detectors_traj"
)
plot!(
map(x->x[1], pos_vect_8_calc),
map(x->x[2], pos_vect_8_calc),
color=:blue, label="traj",
legend=:bottomleft,
legendfont = font("Times New Roman", 12),
titlefont = font("Times New Roman", 14),
title = "Траектории ЦМ при движении по KALMAN, 2D"
)
Out[0]:
In [ ]:
plot(map(x-> sqrt(sum(x.^2)), (pos_vect_8 .- pos_vect_8_calc)), title="Отклонение от траектории при движении по KALMAN", legend = false,
titlefont = font("Times New Roman", 14))
Out[0]:
In [ ]:
plot(map(x-> sqrt(x[1]^2 + x[2]^2), (pos_vect_8 .- pos_vect_8_calc)), title="Отклонение от траектории при движении по KALMAN, 2D", legend = false,
titlefont = font("Times New Roman", 14))
Out[0]: