PX4 Firmware
PX4 Autopilot Software http://px4.io
terrain_estimator.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Roman Bapst. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file terrain_estimator.h
36  */
37 
38 #pragma once
39 
40 #include <lib/mathlib/mathlib.h>
41 #include <matrix/math.hpp>
46 
47 
48 /*
49 * This class can be used to estimate distance to the ground using a laser range finder.
50 * It's assumed that the laser points down vertically if the vehicle is in it's neutral pose.
51 * The predict(...) function will do a state prediciton based on accelerometer inputs. It also
52 * considers accelerometer bias.
53 * The measurement_update(...) function does a measurement update based on range finder and gps
54 * velocity measurements. Both functions should always be called together when there is new
55 * acceleration data available.
56 * The is_valid() function provides information whether the estimate is valid.
57 */
58 
60 {
61 public:
63  ~TerrainEstimator() = default;
64 
65  bool is_valid() {return _terrain_valid;}
66  float get_distance_to_ground() {return -_x(0);}
67  float get_velocity() {return _x(1);}
68 
69  void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor,
70  const struct distance_sensor_s *distance);
71  void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
72  const struct distance_sensor_s *distance,
73  const struct vehicle_attitude_s *attitude);
74 
75 private:
76  enum {n_x = 3};
77 
80 
81  // kalman filter variables
82  matrix::Vector<float, n_x> _x; // state: ground distance, velocity, accel bias in z direction
83  float _u_z; // acceleration in earth z direction
84  matrix::Matrix<float, 3, 3> _P; // covariance matrix
85 
86  // timestamps
88  uint64_t _time_last_gps;
89 
90  /*
91  struct {
92  float var_acc_z;
93  float var_p_z;
94  float var_p_vz;
95  float var_lidar;
96  float var_gps_vz;
97  } _params;
98  */
99 
100  bool is_distance_valid(float distance);
101 
102 };
matrix::Vector< float, n_x > _x
Definition: I2C.hpp:51
uint64_t _time_last_distance
float get_distance_to_ground()
matrix::Matrix< float, 3, 3 > _P
float dt
Definition: px4io.c:73