ESP-IDF Firmware
Firmware architecture and call graph
Loading...
Searching...
No Matches
ekf Class Referenceabstract

#include <ekf.h>

Inheritance diagram for ekf:
[legend]
Collaboration diagram for ekf:
[legend]

Public Member Functions

 ekf (int x, int w)
virtual ~ekf ()
virtual void Process (float *u, float dt)
virtual void Init ()=0
void RungeKutta (dspm::Mat &x, float *u, float dt)
virtual dspm::Mat StateXdot (dspm::Mat &x, float *u)
virtual void LinearizeFG (dspm::Mat &x, float *u)=0
virtual void CovariancePrediction (float dt)
virtual void Update (dspm::Mat &H, float *measured, float *expected, float *R)
virtual void UpdateRef (dspm::Mat &H, float *measured, float *expected, float *R)

Static Public Member Functions

static dspm::Mat quat2rotm (float q[4])
static dspm::Mat rotm2quat (dspm::Mat &R)
static dspm::Mat quat2eul (const float q[4])
static dspm::Mat eul2rotm (float xyz[3])
static dspm::Mat rotm2eul (dspm::Mat &rotm)
static dspm::Mat dFdq (dspm::Mat &vector, dspm::Mat &quat)
static dspm::Mat dFdq_inv (dspm::Mat &vector, dspm::Mat &quat)
static dspm::Mat SkewSym4x4 (float *w)
static dspm::Mat qProduct (float *q)

Data Fields

int NUMX
int NUMW
dspm::Mat & X
dspm::Mat & F
dspm::Mat & G
dspm::Mat & P
dspm::Mat & Q
float * HP
float * Km

Detailed Description

The ekf is a base class for Extended Kalman Filter. It contains main matrix operations and define the processing flow.

Definition at line 29 of file ekf.h.

Constructor & Destructor Documentation

◆ ekf()

ekf::ekf ( int x,
int w )

Constructor of EKF. THe constructor allocate main memory for the matrixes.

Parameters
[in]x- amount of states in EKF. x[n] = F*x[n-1] + G*u + W. Size of matrix F
[in]w- amount of control measurements and noise inputs. Size of matrix G

Definition at line 19 of file ekf.cpp.

19 : NUMX(x),
20 NUMW(w),
21 X(*new dspm::Mat(x, 1)),
22
23 F(*new dspm::Mat(x, x)),
24 G(*new dspm::Mat(x, w)),
25 P(*new dspm::Mat(x, x)),
26 Q(*new dspm::Mat(w, w))
27{
28
29 this->P *= 0;
30 this->Q *= 0;
31 this->X *= 0;
32 this->X.data[0] = 1; // direction to 0
33 this->HP = new float[this->NUMX];
34 this->Km = new float[this->NUMX];
35 for (size_t i = 0; i < this->NUMX; i++) {
36 this->HP[i] = 0;
37 this->Km[i] = 0;
38 }
39}
ekf::NUMW
int NUMW
Definition ekf.h:68
ekf::F
dspm::Mat & F
Definition ekf.h:78
ekf::HP
float * HP
Definition ekf.h:157
ekf::NUMX
int NUMX
Definition ekf.h:63
ekf::G
dspm::Mat & G
Definition ekf.h:82
ekf::X
dspm::Mat & X
Definition ekf.h:73
ekf::Q
dspm::Mat & Q
Definition ekf.h:92
ekf::P
dspm::Mat & P
Definition ekf.h:87
ekf::Km
float * Km
Definition ekf.h:161
x
float x[1024]
Definition test_fir.c:10

References F, G, HP, Km, NUMW, NUMX, P, Q, X, and x.

Referenced by ekf_imu13states::ekf_imu13states().

Here is the caller graph for this function:

◆ ~ekf()

ekf::~ekf ( )
virtual

Distructor of EKF

Definition at line 41 of file ekf.cpp.

42{
43 delete &X;
44 delete &F;
45 delete &G;
46 delete &P;
47 delete &Q;
48
49 delete this->HP;
50 delete this->Km;
51}

References F, G, HP, Km, P, Q, and X.

Member Function Documentation

◆ CovariancePrediction()

void ekf::CovariancePrediction ( float dt)
virtual

Calculates covariance prediction matrux P. Update matrix P

Parameters
[in]dttime interval from last update

Definition at line 139 of file ekf.cpp.

140{
141 dspm::Mat f = this->F * dt;
142
143 f = f + dspm::Mat::eye(this->NUMX);
144
145 dspm::Mat f_t = f.t();
146 this->P = ((f * this->P) * f_t) + (dt * dt) * ((G * Q) * G.t());
147}
dspm::Mat::eye
static Mat eye(int size)
Definition mat.cpp:394
dspm::Mat::t
Mat t()
Definition mat.cpp:383

References dspm::Mat::eye(), F, G, NUMX, P, Q, and dspm::Mat::t().

Referenced by Process().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ dFdq()

dspm::Mat ekf::dFdq ( dspm::Mat & vector,
dspm::Mat & quat )
static

Df/dq: Derivative of vector by quaternion.

Parameters
[in]vectorinput vector
[in]quatquaternion
Returns
  • Derivative matrix 3x4

Definition at line 367 of file ekf.cpp.

368{
369 dspm::Mat result(3, 4);
370 result(0, 0) = q.data[0] * vector.data[0] - q.data[3] * vector.data[1] + q.data[2] * vector.data[2];
371 result(0, 1) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
372 result(0, 2) = -q.data[2] * vector.data[0] + q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
373 result(0, 3) = -q.data[3] * vector.data[0] - q.data[0] * vector.data[1] + q.data[1] * vector.data[2];
374
375 result(1, 0) = q.data[3] * vector.data[0] + q.data[0] * vector.data[1] - q.data[1] * vector.data[2];
376 result(1, 1) = q.data[2] * vector.data[0] - q.data[1] * vector.data[1] - q.data[0] * vector.data[2];
377 result(1, 2) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
378 result(1, 3) = q.data[0] * vector.data[0] - q.data[3] * vector.data[1] + q.data[2] * vector.data[2];
379
380 result(2, 0) = -q.data[2] * vector.data[0] + q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
381 result(2, 1) = q.data[3] * vector.data[0] + q.data[0] * vector.data[1] - q.data[1] * vector.data[2];
382 result(2, 2) = -q.data[0] * vector.data[0] + q.data[3] * vector.data[1] - q.data[2] * vector.data[2];
383 result(2, 3) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
384
385 result *= 2;
386 return result;
387}
dspm::Mat::data
float * data
Definition mat.h:37

References dspm::Mat::data.

◆ dFdq_inv()

dspm::Mat ekf::dFdq_inv ( dspm::Mat & vector,
dspm::Mat & quat )
static

Df/dq: Derivative of vector by inverted quaternion.

Parameters
[in]vectorinput vector
[in]quatquaternion
Returns
  • Derivative matrix 3x4

Definition at line 389 of file ekf.cpp.

390{
391 dspm::Mat result(3, 4);
392 result(0, 0) = q.data[0] * vector.data[0] + q.data[3] * vector.data[1] - q.data[2] * vector.data[2];
393 result(0, 1) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
394 result(0, 2) = -q.data[2] * vector.data[0] + q.data[1] * vector.data[1] - q.data[0] * vector.data[2];
395 result(0, 3) = -q.data[3] * vector.data[0] + q.data[0] * vector.data[1] + q.data[1] * vector.data[2];
396
397 result(1, 0) = -q.data[3] * vector.data[0] + q.data[0] * vector.data[1] + q.data[1] * vector.data[2];
398 result(1, 1) = q.data[2] * vector.data[0] - q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
399 result(1, 2) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
400 result(1, 3) = -q.data[0] * vector.data[0] - q.data[3] * vector.data[1] + q.data[2] * vector.data[2];
401
402 result(2, 0) = q.data[2] * vector.data[0] - q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
403 result(2, 1) = q.data[3] * vector.data[0] - q.data[0] * vector.data[1] - q.data[1] * vector.data[2];
404 result(2, 2) = q.data[0] * vector.data[0] + q.data[3] * vector.data[1] - q.data[2] * vector.data[2];
405 result(2, 3) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
406
407 result *= 2;
408 return result;
409}

References dspm::Mat::data.

Referenced by ekf_imu13states::UpdateRefMeasurement(), ekf_imu13states::UpdateRefMeasurement(), and ekf_imu13states::UpdateRefMeasurementMagn().

Here is the caller graph for this function:

◆ eul2rotm()

dspm::Mat ekf::eul2rotm ( float xyz[3])
static

Convert Euler angels to rotation matrix.

Parameters
[in]xyzEuler angels
Returns
  • rotation matrix 3x3

Definition at line 251 of file ekf.cpp.

252{
253 dspm::Mat result(3, 3);
254 float Cx = std::cos(xyz[0]);
255 float Sx = std::sin(xyz[0]);
256 float Cy = std::cos(xyz[1]);
257 float Sy = std::sin(xyz[1]);
258 float Cz = std::cos(xyz[2]);
259 float Sz = std::sin(xyz[2]);
260
261 result(0, 0) = Cy * Cz;
262 result(0, 1) = -Cy * Sz;
263 result(0, 2) = Sy;
264
265 result(1, 0) = Cz * Sx * Sy + Cx * Sz;
266 result(1, 1) = Cx * Cz - Sx * Sy * Sz;
267 result(1, 2) = -Cy * Sx;
268
269 result(2, 0) = -Cx * Cz * Sy + Sx * Sz;
270 result(2, 1) = Cz * Sx + Cx * Sy * Sz;
271 result(2, 2) = Cx * Cy;
272 return result;
273}

Referenced by draw_3d_image(), draw_3d_image(), draw_3d_image(), draw_3d_image(), ekf_imu13states::TestFull(), and update_rotation_matrix().

Here is the caller graph for this function:

◆ Init()

virtual void ekf::Init ( )
pure virtual

Initialization of EKF. The method should be called befare the first use of the filter.

Implemented in ekf_imu13states.

◆ LinearizeFG()

virtual void ekf::LinearizeFG ( dspm::Mat & x,
float * u )
pure virtual

Calculation of system state matrices F and G

Parameters
[in]xstate vector
[in]ucontrol measurement

Implemented in ekf_imu13states.

References x.

Referenced by Process().

Here is the caller graph for this function:

◆ Process()

void ekf::Process ( float * u,
float dt )
virtual

Main processing method of the EKF.

Parameters
[in]u- input measurements
[in]dt- time difference from the last call in seconds

Definition at line 53 of file ekf.cpp.

54{
55 this->LinearizeFG(this->X, (float *)u);
56 this->RungeKutta(this->X, u, dt);
57 this->CovariancePrediction(dt);
58}
ekf::LinearizeFG
virtual void LinearizeFG(dspm::Mat &x, float *u)=0
ekf::CovariancePrediction
virtual void CovariancePrediction(float dt)
Definition ekf.cpp:139
ekf::RungeKutta
void RungeKutta(dspm::Mat &x, float *u, float dt)
Definition ekf.cpp:60

References CovariancePrediction(), LinearizeFG(), RungeKutta(), and X.

Referenced by ekf_imu13states::TestFull().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ qProduct()

dspm::Mat ekf::qProduct ( float * q)
static

Make right quaternion-product matrices.

Parameters
[in]qsource quaternion
Returns
  • right quaternion-product matrix 4x4

Definition at line 112 of file ekf.cpp.

113{
114 dspm::Mat result(4, 4);
115
116 result.data[0] = q[0];
117 result.data[1] = -q[1];
118 result.data[2] = -q[2];
119 result.data[3] = -q[3];
120
121 result.data[4] = q[1];
122 result.data[5] = q[0];
123 result.data[6] = -q[3];
124 result.data[7] = q[2];
125
126 result.data[8] = q[2];
127 result.data[9] = q[3];
128 result.data[10] = q[0];
129 result.data[11] = -q[1];
130
131 result.data[12] = q[3];
132 result.data[13] = -q[2];
133 result.data[14] = q[1];
134 result.data[15] = q[0];
135
136 return result;
137}

References dspm::Mat::data.

Referenced by ekf_imu13states::LinearizeFG().

Here is the caller graph for this function:

◆ quat2eul()

dspm::Mat ekf::quat2eul ( const float q[4])
static

Convert quaternion to Euler angels.

Parameters
[in]qquaternion
Returns
  • Euler angels 3x1

Definition at line 230 of file ekf.cpp.

231{
232 dspm::Mat result(3, 1);
233 float R13, R11, R12, R23, R33;
234 float q0s = q[0] * q[0];
235 float q1s = q[1] * q[1];
236 float q2s = q[2] * q[2];
237 float q3s = q[3] * q[3];
238
239 R13 = 2.0f * (q[1] * q[3] + q[0] * q[2]);
240 R11 = q0s + q1s - q2s - q3s;
241 R12 = -2.0f * (q[1] * q[2] - q[0] * q[3]);
242 R23 = -2.0f * (q[2] * q[3] - q[0] * q[1]);
243 R33 = q0s - q1s - q2s + q3s;
244
245 result.data[1] = (asinf(R13));
246 result.data[2] = (atan2f(R12, R11));
247 result.data[0] = (atan2f(R23, R33));
248 return result;
249}

References dspm::Mat::data.

◆ quat2rotm()

dspm::Mat ekf::quat2rotm ( float q[4])
static

Convert quaternion to rotation matrix.

Parameters
[in]qquaternion
Returns
  • rotation matrix 3x3

Definition at line 209 of file ekf.cpp.

210{
211 float q0 = q[0];
212 float q1 = q[1];
213 float q2 = q[2];
214 float q3 = q[3];
215 dspm::Mat Rm(3, 3);
216
217 Rm(0, 0) = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
218 Rm(1, 0) = 2.0f * (q1 * q2 + q0 * q3);
219 Rm(2, 0) = 2.0f * (q1 * q3 - q0 * q2);
220 Rm(0, 1) = 2.0f * (q1 * q2 - q0 * q3);
221 Rm(1, 1) = (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3);
222 Rm(2, 1) = 2.0f * (q2 * q3 + q0 * q1);
223 Rm(0, 2) = 2.0f * (q1 * q3 + q0 * q2);
224 Rm(1, 2) = 2.0f * (q2 * q3 - q0 * q1);
225 Rm(2, 2) = (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
226
227 return Rm;
228}

Referenced by draw_3d_image(), draw_3d_image(), draw_3d_image(), draw_3d_image(), draw_3d_image_task(), draw_3d_image_task(), ekf_imu13states::LinearizeFG(), ekf_imu13states::UpdateRefMeasurement(), ekf_imu13states::UpdateRefMeasurement(), and ekf_imu13states::UpdateRefMeasurementMagn().

Here is the caller graph for this function:

◆ rotm2eul()

dspm::Mat ekf::rotm2eul ( dspm::Mat & rotm)
static

Convert rotation matrix to Euler angels.

Parameters
[in]rotmrotation matrix
Returns
  • Euler angels 3x1

Definition at line 279 of file ekf.cpp.

280{
281 dspm::Mat result(3, 1);
282 float x, y, z;
283// float cy = sqrtf(rotm(2, 2) * rotm(2, 2) + rotm(2, 0) * rotm(2, 0));
284 float cy = sqrtf(rotm(2, 2) * rotm(2, 2) + rotm(1, 2) * rotm(1, 2));
285 if (cy > 16 * FLT_EPSILON) {
286 x = -atan2f(rotm(1, 2), rotm(2, 2));
287 y = -atan2f(-rotm(0, 2), (float)cy);
288 z = -atan2f(rotm(0, 1), rotm(0, 0));
289 } else {
290 z = -atan2f(-rotm(1, 0), rotm(1, 1));
291 y = -atan2f(-rotm(0, 2), (float)cy);
292 x = 0;
293 }
294 result(0, 0) = x;
295 result(1, 0) = y;
296 result(2, 0) = z;
297 return result;
298}
FLT_EPSILON
#define FLT_EPSILON
Definition ekf.cpp:276
y
float y[1024]
Definition test_fir.c:11

References FLT_EPSILON, x, and y.

Referenced by draw_3d_image(), draw_3d_image(), draw_3d_image(), draw_3d_image(), draw_3d_image_task(), and draw_3d_image_task().

Here is the caller graph for this function:

◆ rotm2quat()

dspm::Mat ekf::rotm2quat ( dspm::Mat & R)
static

Convert rotation matrix to quaternion.

Parameters
[in]Rrotation matrix
Returns
  • quaternion 4x1

Definition at line 305 of file ekf.cpp.

306{
307 float r11 = m(0, 0);
308 float r12 = m(0, 1);
309 float r13 = m(0, 2);
310 float r21 = m(1, 0);
311 float r22 = m(1, 1);
312 float r23 = m(1, 2);
313 float r31 = m(2, 0);
314 float r32 = m(2, 1);
315 float r33 = m(2, 2);
316 float q0 = (r11 + r22 + r33 + 1.0f) / 4.0f;
317 float q1 = (r11 - r22 - r33 + 1.0f) / 4.0f;
318 float q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
319 float q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
320 if (q0 < 0.0f) {
321 q0 = 0.0f;
322 }
323 if (q1 < 0.0f) {
324 q1 = 0.0f;
325 }
326 if (q2 < 0.0f) {
327 q2 = 0.0f;
328 }
329 if (q3 < 0.0f) {
330 q3 = 0.0f;
331 }
332 q0 = sqrt(q0);
333 q1 = sqrt(q1);
334 q2 = sqrt(q2);
335 q3 = sqrt(q3);
336 if (q0 >= q1 && q0 >= q2 && q0 >= q3) {
337 q0 *= +1.0f;
338 q1 *= SIGN(r32 - r23);
339 q2 *= SIGN(r13 - r31);
340 q3 *= SIGN(r21 - r12);
341 } else if (q1 >= q0 && q1 >= q2 && q1 >= q3) {
342 q0 *= SIGN(r32 - r23);
343 q1 *= +1.0f;
344 q2 *= SIGN(r21 + r12);
345 q3 *= SIGN(r13 + r31);
346 } else if (q2 >= q0 && q2 >= q1 && q2 >= q3) {
347 q0 *= SIGN(r13 - r31);
348 q1 *= SIGN(r21 + r12);
349 q2 *= +1.0f;
350 q3 *= SIGN(r32 + r23);
351 } else if (q3 >= q0 && q3 >= q1 && q3 >= q2) {
352 q0 *= SIGN(r21 - r12);
353 q1 *= SIGN(r31 + r13);
354 q2 *= SIGN(r32 + r23);
355 q3 *= +1.0f;
356 }
357
358 dspm::Mat res(4, 1);
359 res(0, 0) = q0;
360 res(1, 0) = q1;
361 res(2, 0) = q2;
362 res(3, 0) = q3;
363 res /= res.norm();
364 return res;
365}
SIGN
static float SIGN(float x)
Definition ekf.cpp:300
m
const int m
Definition test_mmult.c:16

References m, dspm::Mat::norm(), and SIGN().

Referenced by ekf_imu13states::TestFull().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ RungeKutta()

void ekf::RungeKutta ( dspm::Mat & x,
float * u,
float dt )

Runge-Kutta state update method. The method calculates derivatives of input vector x and control measurements u

Parameters
[in]xstate vector
[in]ucontrol measurement
[in]dttime interval from last update in seconds

Definition at line 60 of file ekf.cpp.

61{
62
63 float dt2 = dt / 2.0f;
64
65 dspm::Mat Xlast = x; // make a working copy
66 dspm::Mat K1 = StateXdot(x, U); // k1 = f(x, u)
67 x = Xlast + (K1 * dt2);
68
69 dspm::Mat K2 = StateXdot(x, U); // k2 = f(x + 0.5*dT*k1, u)
70 x = Xlast + K2 * dt2;
71
72 dspm::Mat K3 = StateXdot(x, U); // k3 = f(x + 0.5*dT*k2, u)
73 x = Xlast + K3 * dt;
74
75 dspm::Mat K4 = StateXdot(x, U); // k4 = f(x + dT * k3, u)
76
77 // Xnew = X + dT * (k1 + 2 * k2 + 2 * k3 + k4) / 6
78 x = Xlast + (K1 + 2.0f * K2 + 2.0f * K3 + K4) * (dt / 6.0f);
79}
ekf::StateXdot
virtual dspm::Mat StateXdot(dspm::Mat &x, float *u)
Definition ekf.cpp:411

References StateXdot(), and x.

Referenced by Process().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ SkewSym4x4()

dspm::Mat ekf::SkewSym4x4 ( float * w)
static

Make skew-symmetric matrix of vector.

Parameters
[in]wsource vector
Returns
  • skew-symmetric matrix 4x4

Definition at line 81 of file ekf.cpp.

82{
83 //={ 0, -w[0], -w[1], -w[2],
84 // w[0], 0, w[2], -w[1],
85 // w[1], -w[2], 0, w[0],
86 // w[2], w[1], -w[0], 0 };
87
88 dspm::Mat result(4, 4);
89 result.data[0] = 0;
90 result.data[1] = -w[0];
91 result.data[2] = -w[1];
92 result.data[3] = -w[2];
93
94 result.data[4] = w[0];
95 result.data[5] = 0;
96 result.data[6] = w[2];
97 result.data[7] = -w[1];
98
99 result.data[8] = w[1];
100 result.data[9] = -w[2];
101 result.data[10] = 0;
102 result.data[11] = w[0];
103
104 result.data[12] = w[2];
105 result.data[13] = w[1];
106 result.data[14] = -w[0];
107 result.data[15] = 0;
108
109 return result;
110}

References dspm::Mat::data.

Referenced by ekf_imu13states::LinearizeFG(), and ekf_imu13states::StateXdot().

Here is the caller graph for this function:

◆ StateXdot()

dspm::Mat ekf::StateXdot ( dspm::Mat & x,
float * u )
virtual

Derivative of state vector X The default calculation: xDot = F*x + G*u It's possible to implement optimized version

Parameters
[in]xstate vector
[in]ucontrol measurement
Returns
xDot - derivative of input vector x and u

Reimplemented in ekf_imu13states.

Definition at line 411 of file ekf.cpp.

412{
413 dspm::Mat U(u, this->G.cols, 1);
414 dspm::Mat Xdot = (this->F * x + this->G * U);
415 return Xdot;
416}

References F, G, and x.

Referenced by RungeKutta().

Here is the caller graph for this function:

◆ Update()

void ekf::Update ( dspm::Mat & H,
float * measured,
float * expected,
float * R )
virtual

Update of current state by measured values. Optimized method for non correlated values Calculate Kalman gain and update matrix P and vector X.

Parameters
[in]Hderivative matrix
[in]measuredarray of measured values
[in]expectedarray of expected values
[in]Rmeasurement noise covariance values

Definition at line 149 of file ekf.cpp.

150{
151 float HPHR, Error;
152 dspm::Mat Y(measured, H.rows, 1);
153 dspm::Mat Z(expected, H.rows, 1);
154
155 for (int m = 0; m < H.rows; m++) {
156 for (int j = 0; j < this->NUMX; j++) {
157 // Find Hp = H*P
158 HP[j] = 0;
159 }
160 for (int k = 0; k < this->NUMX; k++) {
161 for (int j = 0; j < this->NUMX; j++) {
162 // Find Hp = H*P
163 HP[j] += H(m, k) * P(k, j);
164 }
165 }
166 HPHR = R[m]; // Find HPHR = H*P*H' + R
167 for (int k = 0; k < this->NUMX; k++) {
168 HPHR += HP[k] * H(m, k);
169 }
170 float invHPHR = 1.0f / HPHR;
171 for (int k = 0; k < this->NUMX; k++) {
172 Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
173 }
174 for (int i = 0; i < this->NUMX; i++) {
175 // Find P(m)= P(m-1) + K*HP
176 for (int j = i; j < NUMX; j++) {
177 P(i, j) = P(j, i) = P(i, j) - Km[i] * HP[j];
178 }
179 }
180
181 Error = Y(m, 0) - Z(m, 0);
182 for (int i = 0; i < this->NUMX; i++) {
183 // Find X(m)= X(m-1) + K*Error
184 X(i, 0) = X(i, 0) + Km[i] * Error;
185 }
186 }
187}
dspm::Mat::rows
int rows
Definition mat.h:33
k
const int k
Definition test_mmult.c:18

References HP, k, Km, m, NUMX, P, dspm::Mat::rows, and X.

Referenced by ekf_imu13states::UpdateRefMeasurement(), ekf_imu13states::UpdateRefMeasurement(), and ekf_imu13states::UpdateRefMeasurementMagn().

Here is the caller graph for this function:

◆ UpdateRef()

void ekf::UpdateRef ( dspm::Mat & H,
float * measured,
float * expected,
float * R )
virtual

Update of current state by measured values. This method just as a reference for research purpose. Not used in real calculations.

Parameters
[in]Hderivative matrix
[in]measuredarray of measured values
[in]expectedarray of expected values
[in]Rmeasurement noise covariance values

Definition at line 189 of file ekf.cpp.

190{
191 dspm::Mat h_t = H.t();
192 dspm::Mat S = H * P * h_t; // +diag(R);
193 for (size_t i = 0; i < H.rows; i++) {
194 S(i, i) += R[i];
195 }
196
197 dspm::Mat S_ = S.pinv(); // 1 / S
198
199 dspm::Mat K = (P * h_t) * S_;
200 this->P = (dspm::Mat::eye(this->NUMX) - K * H) * P;
201
202 dspm::Mat Y(measured, H.rows, 1);
203 dspm::Mat Z(expected, H.rows, 1);
204
205 dspm::Mat Err = Y - Z;
206 this->X += (K * Err);
207}
dspm::Mat::pinv
Mat pinv()
Definition mat.cpp:707
K
#define K
Definition test_mmult.c:14

References dspm::Mat::eye(), K, NUMX, P, dspm::Mat::pinv(), dspm::Mat::rows, dspm::Mat::t(), and X.

Here is the call graph for this function:

Field Documentation

◆ F

dspm::Mat& ekf::F

Linearized system matrices F, where xDot[n] = F*x[n] + G*u + W

Definition at line 78 of file ekf.h.

Referenced by CovariancePrediction(), ekf(), ekf_imu13states::LinearizeFG(), StateXdot(), and ~ekf().

◆ G

dspm::Mat& ekf::G

Linearized system matrices G, where xDot[n] = F*x[n] + G*u + W

Definition at line 82 of file ekf.h.

Referenced by CovariancePrediction(), ekf(), ekf_imu13states::LinearizeFG(), StateXdot(), and ~ekf().

◆ HP

float* ekf::HP

Matrix for intermidieve calculations

Definition at line 157 of file ekf.h.

Referenced by ekf(), Update(), and ~ekf().

◆ Km

float* ekf::Km

Matrix for intermidieve calculations

Definition at line 161 of file ekf.h.

Referenced by ekf(), Update(), and ~ekf().

◆ NUMW

int ekf::NUMW

xDot[n] = F*x[n] + G*u + W The size of G matrix

Definition at line 68 of file ekf.h.

Referenced by ekf().

◆ NUMX

int ekf::NUMX

xDot[n] = F*x[n] + G*u + W Number of states, X is the state vector (size of F matrix)

Definition at line 63 of file ekf.h.

Referenced by CovariancePrediction(), ekf(), ekf_imu13states::StateXdot(), Update(), UpdateRef(), ekf_imu13states::UpdateRefMeasurement(), ekf_imu13states::UpdateRefMeasurement(), and ekf_imu13states::UpdateRefMeasurementMagn().

◆ P

dspm::Mat& ekf::P

Covariance matrix and state vector

Definition at line 87 of file ekf.h.

Referenced by CovariancePrediction(), ekf(), Update(), UpdateRef(), and ~ekf().

◆ Q

dspm::Mat& ekf::Q

Input noise and measurement noise variances

Definition at line 92 of file ekf.h.

Referenced by CovariancePrediction(), ekf(), ekf_imu13states::Init(), and ~ekf().

◆ X

dspm::Mat& ekf::X

System state vector

Definition at line 73 of file ekf.h.

Referenced by ekf(), ekf_imu13states::Init(), Process(), ekf_imu13states::TestFull(), Update(), UpdateRef(), ekf_imu13states::UpdateRefMeasurement(), ekf_imu13states::UpdateRefMeasurement(), ekf_imu13states::UpdateRefMeasurementMagn(), and ~ekf().

The documentation for this class was generated from the following files: