-
Notifications
You must be signed in to change notification settings - Fork 1k
/
mm_flow.c
131 lines (115 loc) · 4.63 KB
/
mm_flow.c
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
/**
* ,---------, ____ _ __
* | ,-^-, | / __ )(_) /_______________ _____ ___
* | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2021 Bitcraze AB
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, in version 3.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "mm_flow.h"
#include "log.h"
// TODO remove the temporary test variables (used for logging)
static float predictedNX;
static float predictedNY;
static float measuredNX;
static float measuredNY;
void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro)
{
// Inclusion of flow measurements in the EKF done by two scalar updates
// ~~~ Camera constants ~~~
// The angle of aperture is guessed from the raw data register and thankfully look to be symmetric
float Npix = 30.0; // [pixels] (same in x and y)
//float thetapix = DEG_TO_RAD * 4.0f; // [rad] (same in x and y)
float thetapix = DEG_TO_RAD * 4.2f;
//~~~ Body rates ~~~
// TODO check if this is feasible or if some filtering has to be done
float omegax_b = gyro->x * DEG_TO_RAD;
float omegay_b = gyro->y * DEG_TO_RAD;
// ~~~ Moves the body velocity into the global coordinate system ~~~
// [bar{x},bar{y},bar{z}]_G = R*[bar{x},bar{y},bar{z}]_B
//
// \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G
// \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G
//
// where \hat{} denotes a basis vector, \dot{} denotes a derivative and
// _G and _B refer to the global/body coordinate systems.
// Modification 1
//dx_g = R[0][0] * S[KC_STATE_PX] + R[0][1] * S[KC_STATE_PY] + R[0][2] * S[KC_STATE_PZ];
//dy_g = R[1][0] * S[KC_STATE_PX] + R[1][1] * S[KC_STATE_PY] + R[1][2] * S[KC_STATE_PZ];
float dx_g = this->S[KC_STATE_PX];
float dy_g = this->S[KC_STATE_PY];
float z_g = 0.0;
// Saturate elevation in prediction and correction to avoid singularities
if ( this->S[KC_STATE_Z] < 0.1f ) {
z_g = 0.1;
} else {
z_g = this->S[KC_STATE_Z];
}
// ~~~ X velocity prediction and update ~~~
// predics the number of accumulated pixels in the x-direction
float omegaFactor = 1.25f;
float hx[KC_STATE_DIM] = {0};
arm_matrix_instance_f32 Hx = {1, KC_STATE_DIM, hx};
predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegaFactor * omegay_b);
measuredNX = flow->dpixelx;
// derive measurement equation with respect to dx (and z?)
hx[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dx_g) / (-z_g * z_g));
hx[KC_STATE_PX] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
//First update
kalmanCoreScalarUpdate(this, &Hx, measuredNX-predictedNX, flow->stdDevX);
// ~~~ Y velocity prediction and update ~~~
float hy[KC_STATE_DIM] = {0};
arm_matrix_instance_f32 Hy = {1, KC_STATE_DIM, hy};
predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegaFactor * omegax_b);
measuredNY = flow->dpixely;
// derive measurement equation with respect to dy (and z?)
hy[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dy_g) / (-z_g * z_g));
hy[KC_STATE_PY] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
// Second update
kalmanCoreScalarUpdate(this, &Hy, measuredNY-predictedNY, flow->stdDevY);
}
/**
* Predicted and measured values of the X and Y direction of the flowdeck
*/
LOG_GROUP_START(kalman_pred)
/**
* @brief Flow sensor predicted dx [pixels/frame]
*
* note: rename to kalmanMM.flowX?
*/
LOG_ADD(LOG_FLOAT, predNX, &predictedNX)
/**
* @brief Flow sensor predicted dy [pixels/frame]
*
* note: rename to kalmanMM.flowY?
*/
LOG_ADD(LOG_FLOAT, predNY, &predictedNY)
/**
* @brief Flow sensor measured dx [pixels/frame]
*
* note: This is the same as motion.deltaX, so perhaps remove this?
*/
LOG_ADD(LOG_FLOAT, measNX, &measuredNX)
/**
* @brief Flow sensor measured dy [pixels/frame]
*
* note: This is the same as motion.deltaY, so perhaps remove this?
*/
LOG_ADD(LOG_FLOAT, measNY, &measuredNY)
LOG_GROUP_STOP(kalman_pred)