Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ekf_range.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
26 #include "ekf_range.h"
27 #include <math.h>
28 
29 void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
30 {
31  int i,j;
32  const float dt2 = dt * dt;
33  const float dt3 = dt2 * dt / 2.f;
34  const float dt4 = dt2 * dt2 / 4.f;
35  for (i = 0; i < EKF_RANGE_DIM; i++) {
36  ekf_range->state[i] = 0.f; // don't forget to call set_state before running the filter for better results
37  for (j = 0; j < EKF_RANGE_DIM; j++) {
38  ekf_range->P[i][j] = 0.f;
39  ekf_range->Q[i][j] = 0.f;
40  }
41  }
42  for (i = 0; i < EKF_RANGE_DIM; i += 2) {
43  ekf_range->P[i][i] = P0_pos;
44  ekf_range->P[i+1][i+1] = P0_speed;
45  ekf_range->Q[i][i] = Q_sigma2 * dt4;
46  ekf_range->Q[i+1][i] = Q_sigma2 * dt3;
47  ekf_range->Q[i][i+1] = Q_sigma2 * dt3;
48  ekf_range->Q[i+1][i+1] = Q_sigma2 * dt2;
49  }
50  ekf_range->R_dist = R_dist;
51  ekf_range->R_speed = R_speed;
52  ekf_range->dt = dt;
53 }
54 
55 void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
56 {
57  ekf_range->state[0] = pos.x;
58  ekf_range->state[1] = speed.x;
59  ekf_range->state[2] = pos.y;
60  ekf_range->state[3] = speed.y;
61  ekf_range->state[4] = pos.z;
62  ekf_range->state[5] = speed.z;
63 }
64 
65 void ekf_range_get_state(struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed)
66 {
67  pos->x = ekf_range->state[0];
68  pos->y = ekf_range->state[2];
69  pos->z = ekf_range->state[4];
70  speed->x = ekf_range->state[1];
71  speed->y = ekf_range->state[3];
72  speed->z = ekf_range->state[5];
73 }
74 
75 struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
76 {
77  struct EnuCoor_f pos;
78  pos.x = ekf_range->state[0];
79  pos.y = ekf_range->state[2];
80  pos.z = ekf_range->state[4];
81  return pos;
82 }
83 
84 struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
85 {
86  struct EnuCoor_f speed;
87  speed.x = ekf_range->state[1];
88  speed.y = ekf_range->state[3];
89  speed.z = ekf_range->state[5];
90  return speed;
91 }
92 
93 void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
94 {
95  int i;
96  const float dt = ekf_range->dt;
97  const float dt2 = dt * dt;
98  const float dt3 = dt2 * dt / 2.f;
99  const float dt4 = dt2 * dt2 / 4.f;
100  for (i = 0; i < EKF_RANGE_DIM; i += 2) {
101  ekf_range->Q[i][i] = Q_sigma2 * dt4;
102  ekf_range->Q[i+1][i] = Q_sigma2 * dt3;
103  ekf_range->Q[i][i+1] = Q_sigma2 * dt3;
104  ekf_range->Q[i+1][i+1] = Q_sigma2 * dt2;
105  }
106  ekf_range->R_dist = R_dist;
107  ekf_range->R_speed = R_speed;
108 }
109 
119 void ekf_range_predict(struct EKFRange *ekf_range)
120 {
121  int i;
122  for (i = 0; i < EKF_RANGE_DIM; i += 2) {
123  // kinematic equation of the dynamic model X = F*X
124  ekf_range->state[i] += ekf_range->state[i+1] * ekf_range->dt;
125  // propagate covariance P = F*P*Ft + Q
126  // since F is diagonal by block, P can be updated by block here as well
127  // let's unroll the matrix operations as it is simple
128  const float d_dt = ekf_range->P[i+1][i+1] * ekf_range->dt;
129  ekf_range->P[i][i] += ekf_range->P[i+1][i] * ekf_range->dt
130  + ekf_range->dt * (ekf_range->P[i][i+1] + d_dt) + ekf_range->Q[i][i];
131  ekf_range->P[i][i+1] += d_dt + ekf_range->Q[i][i+1];
132  ekf_range->P[i+1][i] += d_dt + ekf_range->Q[i+1][i];
133  ekf_range->P[i+1][i+1] += ekf_range->Q[i+1][i+1];
134  }
135 }
136 
143 void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
144 {
145  const float dx = ekf_range->state[0] - anchor.x;
146  const float dy = ekf_range->state[2] - anchor.y;
147  const float dz = ekf_range->state[4] - anchor.z;
148  const float norm = sqrtf(dx * dx + dy * dy + dz * dz);
149  // build measurement error
150  const float res = dist - norm;
151  // build Jacobian of observation model for anchor i
152  float Hi[] = { dx / norm, 0.f, dy / norm, 0.f, dz / norm, 0.f };
153  // compute kalman gain K = P*Ht (H*P*Ht + R)^-1
154  // S = H*P*Ht + R
155  const float S =
156  Hi[0] * Hi[0] * ekf_range->P[0][0] +
157  Hi[2] * Hi[2] * ekf_range->P[2][2] +
158  Hi[4] * Hi[4] * ekf_range->P[4][4] +
159  Hi[0] * Hi[2] * (ekf_range->P[0][2] + ekf_range->P[2][0]) +
160  Hi[0] * Hi[4] * (ekf_range->P[0][4] + ekf_range->P[4][0]) +
161  Hi[2] * Hi[4] * (ekf_range->P[2][4] + ekf_range->P[4][2]) +
162  ekf_range->R_dist;
163  if (fabsf(S) < 1e-5) {
164  return; // don't inverse S if it is too small
165  }
166  // finally compute gain and correct state
167  float K[6];
168  for (int i = 0; i < 6; i++) {
169  K[i] = (Hi[0] * ekf_range->P[i][0] + Hi[2] * ekf_range->P[i][2] + Hi[4] * ekf_range->P[i][4]) / S;
170  ekf_range->state[i] += K[i] * res;
171  }
172  // precompute K*H and store current P
173  float KH_tmp[6][6];
174  float P_tmp[6][6];
175  for (int i = 0; i < 6; i++) {
176  for (int j = 0; j < 6; j++) {
177  KH_tmp[i][j] = K[i] * Hi[j];
178  P_tmp[i][j] = ekf_range->P[i][j];
179  }
180  }
181  // correct covariance P = (I-K*H)*P = P - K*H*P
182  for (int i = 0; i < 6; i++) {
183  for (int j = 0; j < 6; j++) {
184  for (int k = 0; k < 6; k++) {
185  ekf_range->P[i][j] -= KH_tmp[i][k] * P_tmp[k][j];
186  }
187  }
188  }
189 }
190 
191 void ekf_range_update_speed(struct EKFRange *ekf_range, float speed, uint8_t type)
192 {
193  (void) ekf_range;
194  (void) speed;
195  (void) type;
196  // TODO
197 }
198 
if(GpsFixValid() &&e_identification_started)
void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
Update process and measurement noises.
Definition: ekf_range.c:93
void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
Set initial state vector.
Definition: ekf_range.c:55
struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
Get current speed.
Definition: ekf_range.c:84
void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
correction step
Definition: ekf_range.c:143
void ekf_range_update_speed(struct EKFRange *ekf_range, float speed, uint8_t type)
Update step based on speed measure.
Definition: ekf_range.c:191
void ekf_range_get_state(struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed)
Get current state.
Definition: ekf_range.c:65
struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
Get current pos.
Definition: ekf_range.c:75
void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
Init EKF_range internal struct.
Definition: ekf_range.c:29
void ekf_range_predict(struct EKFRange *ekf_range)
propagate dynamic model
Definition: ekf_range.c:119
float state[EKF_RANGE_DIM]
state vector
Definition: ekf_range.h:44
#define EKF_RANGE_DIM
Definition: ekf_range.h:33
float R_dist
measurement noise on distances (assumed the same for all anchors)
Definition: ekf_range.h:47
float P[EKF_RANGE_DIM][EKF_RANGE_DIM]
covariance matrix
Definition: ekf_range.h:45
float R_speed
measurement noise on speed (assumed the same for all axis)
Definition: ekf_range.h:48
float Q[EKF_RANGE_DIM][EKF_RANGE_DIM]
process noise matrix
Definition: ekf_range.h:46
float dt
prediction step (in seconds)
Definition: ekf_range.h:49
EKF_range structure.
Definition: ekf_range.h:43
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
static float K[9]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98