lidar.cpp
3.35 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
122
123
124
125
126
127
128
129
130
131
132
133
134
#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>
extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
//
static const uint32_t REQ_LIDAR_INIT_COUNT = 10;
static const uint32_t LIDAR_TIMEOUT = 1000000; // 1.0 s
void BlockLocalPositionEstimator::lidarInit()
{
// measure
Vector<float, n_y_lidar> y;
if (lidarMeasure(y) != OK) {
_lidarStats.reset();
}
// if finished
if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) {
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar init: "
"mean %d cm stddev %d cm",
int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_LIDAR;
_sensorFault &= ~SENSOR_LIDAR;
}
}
int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
{
// measure
float d = _sub_lidar->get().current_distance;
float eps = 0.01f; // 1 cm
float min_dist = _sub_lidar->get().min_distance + eps;
float max_dist = _sub_lidar->get().max_distance - eps;
// prevent driver from setting min dist below eps
if (min_dist < eps) {
min_dist = eps;
}
// check for bad data
if (d > max_dist || d < min_dist) {
return -1;
}
// update stats
_lidarStats.update(Scalarf(d));
_time_last_lidar = _timeStamp;
y.setZero();
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
y(0) = (d + _param_lpe_ldr_off_z.get()) *
cosf(euler.phi()) *
cosf(euler.theta());
return OK;
}
void BlockLocalPositionEstimator::lidarCorrect()
{
// measure lidar
Vector<float, n_y_lidar> y;
if (lidarMeasure(y) != OK) { return; }
// measurement matrix
Matrix<float, n_y_lidar, n_x> C;
C.setZero();
// y = -(z - tz)
// TODO could add trig to make this an EKF correction
C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir.
C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir.
// use parameter covariance unless sensor provides reasonable value
SquareMatrix<float, n_y_lidar> R;
R.setZero();
float cov = _sub_lidar->get().variance;
if (cov < 1.0e-3f) {
R(0, 0) = _param_lpe_ldr_z.get() * _param_lpe_ldr_z.get();
} else {
R(0, 0) = cov;
}
// residual
Vector<float, n_y_lidar> r = y - C * _x;
// residual covariance
Matrix<float, n_y_lidar, n_y_lidar> S = C * m_P * C.transpose() + R;
// publish innovations
_pub_innov.get().hagl = r(0);
_pub_innov_var.get().hagl = S(0, 0);
// residual covariance, (inverse)
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>(S);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_lidar]) {
if (!(_sensorFault & SENSOR_LIDAR)) {
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_LIDAR;
}
// abort correction
return;
} else if (_sensorFault & SENSOR_LIDAR) {
_sensorFault &= ~SENSOR_LIDAR;
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar OK");
}
// kalman filter correction always
Matrix<float, n_x, n_y_lidar> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::lidarCheckTimeout()
{
if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_LIDAR)) {
_sensorTimeout |= SENSOR_LIDAR;
_lidarStats.reset();
mavlink_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
}
}
}