def calc_anj_cov_matrix(self) -> None:
"""Расчёт настоящей ковариационной матрицы АШП в декартовых координатах,
как суммы матриц ковариаций измерений и матрицы ковариаций метода
:return: None
"""
# Матрица ошибок метода в сферических координатах
method_cov_matrix_sph = self.calculate_method_cov_matrix_for_jammer()
# Матрица ошибок измерений в сферических координатах
measure_cov_matrix_sph = dec2sph_cov_matrix(self.jammer_trace.coordinate_covariance_matrix, self.jammer_trace.coordinates)
# Необходимое обнуление некоторых элементов
self.make_zero_elements_associated_with_range(measure_cov_matrix_sph)
# Итоговая матрица ошибок в сферических координатах
cov_matrix_sph = measure_cov_matrix_sph + method_cov_matrix_sph
# Координаты АШП в сферических координатах
coords_sph = dec2sph(self.est_coordinates_anj - self.jammer_trace.mfr_position)
# Итоговая матрица в декартовых координатах
self.real_cov_matrix_anj = sph2dec_cov_matrix(cov_matrix_sph, coords_sph)
def test_dec2sph_and_sph2dec_cov_matrix(self) -> None:
"""Проверка пересчётов ковариационных матриц из декартовых координат в сферические и обратно
:return: None
"""
# Определим нужные для функции данные
coordinate_dec = np.array([4_000., 5_000., 3_000.])
real_covariance_matrix_sph = np.diag([25, 0.01, 1])
coordinate_sph = dec2sph(coordinate_dec)
# Применяем последовательно тестируемые функции
covariance_matrix_dec = sph2dec_cov_matrix(real_covariance_matrix_sph, coordinate_sph)
covariance_matrix_sph = dec2sph_cov_matrix(covariance_matrix_dec, coordinate_dec)
# Проверка для ковариационной матрицы
covariance_matrix_sph = covariance_matrix_sph.round(7).tolist()
real_covariance_matrix_sph = [[25.0, -0.0, 0.0],
[-0.0, 0.01, -0.0],
[0.0, -0.0, 1.0]]
self.assertEqual(real_covariance_matrix_sph, covariance_matrix_sph, "После последовательного пересчета "
"получились разные ковариационные матрицы")
def _calculate_real_covariance_matrix_2(self) -> ndarray:
"""
:return: Настоящая ковариационная матрица ближайшей точки на пеленге от второй трассы
:rtype: ndarray
"""
cov_matrix_2 = self.second_trace.coordinate_covariance_matrix
variance_r2 = self.b * self.variance_t
covariance_r2b2 = sqrt(self.b) * (self.t_derivative_2 @ cov_matrix_2 @ self.beta_derivative_2.T)
covariance_r2e2 = sqrt(self.b) * (self.t_derivative_2 @ cov_matrix_2 @ self.eps_derivative_2.T)
sph_covariance_matrix2 = dec2sph_cov_matrix(cov_matrix_2, self.mfr_anj_2)
# Индексы по соответсвующим сферическим координатам
r2, b2, e2 = 0, 1, 2
sph_covariance_matrix2[r2][r2] = variance_r2
sph_covariance_matrix2[r2][b2] = sph_covariance_matrix2[b2][r2] = covariance_r2b2
sph_covariance_matrix2[r2][e2] = sph_covariance_matrix2[e2][r2] = covariance_r2e2
nearest_point_in_sph_mfr_coord_2 = dec2sph(self.nearest_point_second_bearing - self.second_trace.mfr_position)
real_covariance_matrix_2 = sph2dec_cov_matrix(sph_covariance_matrix2, nearest_point_in_sph_mfr_coord_2)
return real_covariance_matrix_2
def _calculate_real_covariance_matrix_1(self) -> ndarray:
"""
:return: Настоящая ковариационная матрица ближайшей точки на пеленге от первой трассы
:rtype: ndarray
"""
cov_matrix_1 = self.first_trace.coordinate_covariance_matrix
variance_r1 = self.d * self.variance_s
covariance_r1b1 = sqrt(self.d) * (self.s_derivative_1 @ cov_matrix_1 @ self.beta_derivative_1.T)
covariance_r1e1 = sqrt(self.d) * (self.s_derivative_1 @ cov_matrix_1 @ self.eps_derivative_1.T)
sph_covariance_matrix1 = dec2sph_cov_matrix(cov_matrix_1, self.mfr_anj_1)
# Индексы по соответсвующим сферическим координатам
r1, b1, e1 = 0, 1, 2
sph_covariance_matrix1[r1][r1] = variance_r1
sph_covariance_matrix1[r1][b1] = sph_covariance_matrix1[b1][r1] = covariance_r1b1
sph_covariance_matrix1[r1][e1] = sph_covariance_matrix1[e1][r1] = covariance_r1e1
nearest_point_in_sph_mfr_coord_1 = dec2sph(self.nearest_point_first_bearing - self.first_trace.mfr_position)
real_covariance_matrix_1 = sph2dec_cov_matrix(sph_covariance_matrix1, nearest_point_in_sph_mfr_coord_1)
return real_covariance_matrix_1
def estimate_coords_and_covariance_matrix(trace, mean_sph_coord,
sigma):
est_sph_coords = normal(mean_sph_coord, sigma)
trace.coordinates = sph2dec(est_sph_coords) + trace.mfr_position
trace.coordinate_covariance_matrix = sph2dec_cov_matrix(
np.diag(sigma**2), est_sph_coords)
def measure_trace_coordinates(trace, mean_dec_coord, sigma):
mean_sph_coord = dec2sph(mean_dec_coord - trace.mfr_position)
est_sph_coords = normal(mean_sph_coord, sigma)
trace.coordinates = sph2dec(est_sph_coords) + trace.mfr_position
trace.coordinate_covariance_matrix = sph2dec_cov_matrix(
np.diag(sigma**2), est_sph_coords)