Применение триангуляции к модели движения ТС по маршруту
Автор
Δt = bins_st
sygma_xyz_triangulation = 0.0003026362681712997
A = [
1 0 0 Δt 0 0 0 0 0;
0 1 0 0 Δt 0 0 0 0;
0 0 1 0 0 Δt 0 0 0;
0 0 0 1 0 0 0 0 0;
0 0 0 0 1 0 0 0 0;
0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 1;
]
B = [
Δt^2/2 0 0 0 0 0;
0 Δt^2/2 0 0 0 0;
0 0 Δt^2/2 0 0 0;
Δt 0 0 0 0 0;
0 Δt 0 0 0 0;
0 0 Δt 0 0 0;
0 0 0 Δt 0 0;
0 0 0 0 Δt 0;
0 0 0 0 0 Δt;
]
Q = zeros(9, 9)
H = [
1 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 1;
]
R = [
sygma_xyz_triangulation^2 0 0 0 0 0;
0 sygma_xyz_triangulation^2 0 0 0 0;
0 0 sygma_xyz_triangulation^2 0 0 0;
0 0 0 magnetometer_noise_power 0 0;
0 0 0 0 dkt_noise_power 0;
0 0 0 0 0 dkt_noise_power;
]
P0 = [
sygma_xyz_triangulation^2 0 0 0 0 0 0 0 0;
0 sygma_xyz_triangulation^2 0 0 0 0 0 0 0;
0 0 sygma_xyz_triangulation^2 0 0 0 0 0 0;
0 0 0 1e-10 0 0 0 0 0;
0 0 0 0 1e-10 0 0 0 0;
0 0 0 0 0 1e-10 0 0 0;
0 0 0 0 0 0 magnetometer_noise_power 0 0;
0 0 0 0 0 0 0 dkt_noise_power 0;
0 0 0 0 0 0 0 0 dkt_noise_power;
]
Qu = [
accelarations_noise_power 0 0 0 0 0;
0 accelarations_noise_power 0 0 0 0;
0 0 accelarations_noise_power 0 0 0;
0 0 0 magnetometer_noise_power 0 0;
0 0 0 0 dkt_noise_power 0;
0 0 0 0 0 dkt_noise_power;
];
Q = B * Qu * B';