landing_target.cpp
2.96 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>
extern orb_advert_t mavlink_log_pub;
static const uint64_t TARGET_TIMEOUT = 2000000; // [us]
void BlockLocalPositionEstimator::landingTargetInit()
{
if (_param_ltest_mode.get() == Target_Moving) {
// target is in moving mode, do not initialize
return;
}
Vector<float, n_y_target> y;
if (landingTargetMeasure(y) == OK) {
mavlink_log_info(&mavlink_log_pub, "Landing target init");
_sensorTimeout &= ~SENSOR_LAND_TARGET;
_sensorFault &= ~SENSOR_LAND_TARGET;
}
}
int BlockLocalPositionEstimator::landingTargetMeasure(Vector<float, n_y_target> &y)
{
if (_param_ltest_mode.get() == Target_Stationary) {
if (_sub_landing_target_pose.get().rel_vel_valid) {
y(0) = _sub_landing_target_pose.get().vx_rel;
y(1) = _sub_landing_target_pose.get().vy_rel;
_time_last_target = _timeStamp;
} else {
return -1;
}
return OK;
}
return -1;
}
void BlockLocalPositionEstimator::landingTargetCorrect()
{
if (_param_ltest_mode.get() == Target_Moving) {
// nothing to do in this mode
return;
}
// measure
Vector<float, n_y_target> y;
if (landingTargetMeasure(y) != OK) { return; }
// calculate covariance
float cov_vx = _sub_landing_target_pose.get().cov_vx_rel;
float cov_vy = _sub_landing_target_pose.get().cov_vy_rel;
// use sensor value only if reasoanble
if (cov_vx < _param_lpe_lt_cov.get() || cov_vy < _param_lpe_lt_cov.get()) {
cov_vx = _param_lpe_lt_cov.get();
cov_vy = _param_lpe_lt_cov.get();
}
// target measurement matrix and noise matrix
Matrix<float, n_y_target, n_x> C;
C.setZero();
// residual = (y + vehicle velocity)
// sign change because target velocitiy is -vehicle velocity
C(Y_target_x, X_vx) = -1;
C(Y_target_y, X_vy) = -1;
// covariance matrix
SquareMatrix<float, n_y_target> R;
R.setZero();
R(0, 0) = cov_vx;
R(1, 1) = cov_vy;
// residual
Vector<float, n_y_target> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_target, n_y_target> S_I =
inv<float, n_y_target>(C * m_P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_target]) {
if (!(_sensorFault & SENSOR_LAND_TARGET)) {
mavlink_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_LAND_TARGET;
}
// abort correction
return;
} else if (_sensorFault & SENSOR_LAND_TARGET) {
_sensorFault &= ~SENSOR_LAND_TARGET;
mavlink_log_info(&mavlink_log_pub, "Landing target OK");
}
// kalman filter correction
Matrix<float, n_x, n_y_target> K =
m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::landingTargetCheckTimeout()
{
if (_timeStamp - _time_last_target > TARGET_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_LAND_TARGET)) {
_sensorTimeout |= SENSOR_LAND_TARGET;
mavlink_log_info(&mavlink_log_pub, "Landing target timeout");
}
}
}