EKFilter.h
Go to the documentation of this file.
1 // -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
2 #ifndef EKFILTER_H
3 #define EKFILTER_H
4 
5 #include <hrpUtil/EigenTypes.h>
6 #include <iostream>
7 #include "hrpsys/util/Hrpsys.h"
8 
9 namespace hrp{
10  typedef Eigen::Matrix<double, 7, 7> Matrix77;
11  typedef Eigen::Matrix<double, 7, 1> Vector7;
12 };
13 
14 class EKFilter {
15 public:
17  : P(hrp::Matrix77::Identity() * 0.1),
18  Q(Eigen::Matrix3d::Identity() * 0.001),
19  R(Eigen::Matrix3d::Identity() * 0.03),
20  g_vec(Eigen::Vector3d(0.0, 0.0, 9.80665)),
21  z_k(Eigen::Vector3d(0.0, 0.0, 9.80665)),
22  min_mag_thre_acc(0.005), max_mag_thre_acc(0.05),
23  min_mag_thre_gyro(0.0075), max_mag_thre_gyro(0.035)
24  {
25  x << 1, 0, 0, 0, 0, 0, 0;
26  }
27 
28  hrp::Vector7 getx() const { return x; }
29 
30  void calcOmega(Eigen::Matrix4d& omega, const Eigen::Vector3d& w) const {
31  /* \dot{q} = \frac{1}{2} omega q */
32  omega <<
33  0, -w[0], -w[1], -w[2],
34  w[0], 0, w[2], -w[1],
35  w[1], -w[2], 0, w[0],
36  w[2], w[1], -w[0], 0;
37  }
38 
39  void calcPredictedState(hrp::Vector7& _x_a_priori,
40  const Eigen::Vector4d& q,
41  const Eigen::Vector3d& gyro,
42  const Eigen::Vector3d& drift) const {
43  /* x_a_priori = f(x, u) */
44  Eigen::Vector4d q_a_priori;
45  Eigen::Vector3d gyro_compensated = gyro - drift;
46  Eigen::Matrix4d omega;
47  calcOmega(omega, gyro_compensated);
48  q_a_priori = q + dt / 2 * omega * q;
49  _x_a_priori.head<4>() = q_a_priori.normalized();
50  _x_a_priori.tail<3>() = drift;
51  }
52 
54  const Eigen::Vector4d& q,
55  const Eigen::Vector3d& gyro,
56  const Eigen::Vector3d& drift) const {
57  F = hrp::Matrix77::Identity();
58  Eigen::Vector3d gyro_compensated = gyro - drift;
59  Eigen::Matrix4d omega;
60  calcOmega(omega, gyro_compensated);
61  F.block<4, 4>(0, 0) += dt / 2 * omega;
62  F.block<4, 3>(0, 4) <<
63  + q[1], + q[2], + q[3],
64  - q[0], + q[3], - q[2],
65  - q[3], - q[0], + q[1],
66  + q[2], - q[1], - q[0];
67  F.block<4, 3>(0, 4) *= dt / 2;
68  }
69 
71  const hrp::Matrix77& F,
72  const Eigen::Vector4d& q) const {
73  /* P_a_priori = F P F^T + V Q V^T */
74  Eigen::Matrix<double, 4, 3> V_upper;
75  V_upper <<
76  - q[1], - q[2], - q[3],
77  + q[0], - q[3], + q[2],
78  + q[3], + q[0], - q[1],
79  - q[2], + q[1], + q[0];
80  V_upper *= dt / 2;
81  hrp::Matrix77 VQVt = hrp::Matrix77::Zero();
82  VQVt.block<4, 4>(0, 0) = V_upper * Q * V_upper.transpose();
83  _P_a_priori = F * P * F.transpose() + VQVt;
84  }
85 
86  Eigen::Vector3d calcAcc(const Eigen::Vector4d& q) const {
87  Eigen::Quaternion<double> q_tmp(q[0], q[1], q[2], q[3]);
88  Eigen::Vector3d acc = q_tmp.conjugate()._transformVector(g_vec);
89  return acc;
90  }
91 
92  void calcH(Eigen::Matrix<double, 3, 7>& H, const Eigen::Vector4d& q) const {
93  double w = q[0], x = q[1], y = q[2], z = q[3];
94  H <<
95  -y, +z, -w, +x, 0, 0, 0,
96  +x, +w, +z, +y, 0, 0, 0,
97  +w, -x, -y, +z, 0, 0, 0;
98  H *= 2 * g_vec[2];
99  }
100 
101  Eigen::Vector3d calcMeasurementResidual(const Eigen::Vector3d& acc_measured,
102  const Eigen::Vector4d& q) const {
103  /* y = z - h(x) */
104  Eigen::Vector3d y = acc_measured - calcAcc(q);
105  return y;
106  }
107 
108 
109  void prediction(const Eigen::Vector3d& u) {
110  Eigen::Vector4d q = x.head<4>();
111  Eigen::Vector3d drift = x.tail<3>();
112  hrp::Matrix77 F;
113  calcF(F, q, u, drift);
114  hrp::Vector7 x_tmp;
115  calcPredictedState(x_tmp, q, u, drift);
116  x_a_priori = x_tmp;
117  hrp::Matrix77 P_tmp;
118  calcPredictedCovariance(P_tmp, F, q);
119  P_a_priori = P_tmp;
120  }
121 
122  void correction(const Eigen::Vector3d& z, const Eigen::Matrix3d& fuzzyR) {
123  Eigen::Vector4d q_a_priori = x_a_priori.head<4>();
124  Eigen::Matrix<double, 3, 7> H;
125  z_k = z;
126  Eigen::Vector3d y = calcMeasurementResidual(z, q_a_priori);
127  calcH(H, q_a_priori);
128  Eigen::Matrix3d S = H * P_a_priori * H.transpose() + fuzzyR;
129  Eigen::Matrix<double, 7, 3> K = P_a_priori * H.transpose() * S.inverse();
130  hrp::Vector7 x_tmp = x_a_priori + K * y;
131  x.head<4>() = x_tmp.head<4>().normalized(); /* quaternion */
132  x.tail<3>() = x_tmp.tail<3>(); /* bias */
133  P = (hrp::Matrix77::Identity() - K * H) * P_a_priori;
134  }
135 
136  void printAll() const {
137  std::cerr << "x" << std::endl << x << std::endl;
138  std::cerr << "x_a_priori" << std::endl << x_a_priori << std::endl;
139  std::cerr << "P" << std::endl << P << std::endl << std::endl;
140  std::cerr << "P_a_priori" << std::endl << P_a_priori << std::endl << std::endl;
141  }
142 
143 
144  // Basically Equation (23), (24) and (25) in the paper [1]
145  // [1] Chul Woo Kang and Chan Gook Park. Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter.
146  // In European Control Conference, 2009.
147  void calcRWithFuzzyRule(Eigen::Matrix3d& fuzzyR, const hrp::Vector3& acc, const hrp::Vector3& gyro) const {
148  double alpha = std::min(std::fabs(acc.norm() - g_vec.norm()) / g_vec.norm(), 0.1);
149  double beta = std::min(gyro.norm(), 0.05);
150  double large_mu_acc = std::max(std::min((alpha - min_mag_thre_acc) / (max_mag_thre_acc - min_mag_thre_acc), 1.0), 0.0);
151  double large_mu_gyro = std::max(std::min((beta - min_mag_thre_gyro) / (max_mag_thre_gyro - min_mag_thre_gyro), 1.0), 0.0);
152  double w1, w2, w3, w4;
153  w1 = (1.0 - large_mu_acc) * (1.0 - large_mu_gyro);
154  w2 = (1.0 - large_mu_acc) * large_mu_gyro;
155  w3 = large_mu_acc * (1.0 - large_mu_gyro);
156  w4 = large_mu_acc * large_mu_gyro;
157  double z = (w1 * 0.0 + w2 * (3.5 * alpha + 8.0 * beta + 0.5) + w3 * (3.5 * alpha + 8.0 * beta + 0.5) + w4 * 1.0) / (w1 + w2 + w3 + w4);
158  double k1 = 400;
159  fuzzyR = R + k1 * z * z * Eigen::Matrix3d::Identity();
160  };
161 
162  void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, const hrp::Vector3& acc, const hrp::Vector3& gyro)
163  {
164  Eigen::Matrix3d fuzzyR;
165  calcRWithFuzzyRule(fuzzyR, acc, gyro);
166  prediction(gyro);
167  correction(acc, fuzzyR);
168  /* ekf_filter.printAll(); */
169  Eigen::Quaternion<double> q(x[0], x[1], x[2], x[3]);
170  rpy = hrp::rpyFromRot(q.toRotationMatrix());
171  };
172 
173  void setdt (const double _dt) { dt = _dt;};
175  Eigen::Quaternion<double> tmp_q;
176  tmp_q.setFromTwoVectors(z_k, g_vec);
177  x << tmp_q.w(), tmp_q.x(), tmp_q.y(), tmp_q.z(), 0, 0, 0;
178  };
179 private:
182  Eigen::Matrix3d Q, R;
183  Eigen::Vector3d g_vec, z_k;
184  double dt;
185  double min_mag_thre_acc, max_mag_thre_acc, min_mag_thre_gyro, max_mag_thre_gyro;
186 };
187 
188 #endif /* EKFILTER_H */
Eigen::Vector3d z_k
Definition: EKFilter.h:183
void setdt(const double _dt)
Definition: EKFilter.h:173
hrp::Vector7 getx() const
Definition: EKFilter.h:28
double dt
Definition: EKFilter.h:184
#define max(a, b)
hrp::Vector7 x_a_priori
Definition: EKFilter.h:178
double min_mag_thre_gyro
Definition: EKFilter.h:185
Eigen::Matrix< double, 7, 7 > Matrix77
Definition: EKFilter.h:10
void main_one(hrp::Vector3 &rpy, hrp::Vector3 &rpyRaw, const hrp::Vector3 &acc, const hrp::Vector3 &gyro)
Definition: EKFilter.h:162
w
hrp::Matrix77 P_a_priori
Definition: EKFilter.h:181
void correction(const Eigen::Vector3d &z, const Eigen::Matrix3d &fuzzyR)
Definition: EKFilter.h:122
Eigen::Vector3d calcAcc(const Eigen::Vector4d &q) const
Definition: EKFilter.h:86
Eigen::Vector3d Vector3
#define min(a, b)
void calcPredictedCovariance(hrp::Matrix77 &_P_a_priori, const hrp::Matrix77 &F, const Eigen::Vector4d &q) const
Definition: EKFilter.h:70
Eigen::Matrix< double, 7, 1 > Vector7
Definition: EKFilter.h:11
void resetKalmanFilterState()
Definition: EKFilter.h:174
void calcH(Eigen::Matrix< double, 3, 7 > &H, const Eigen::Vector4d &q) const
Definition: EKFilter.h:92
void prediction(const Eigen::Vector3d &u)
Definition: EKFilter.h:109
void printAll() const
Definition: EKFilter.h:136
Eigen::Matrix3d R
Definition: EKFilter.h:182
void calcPredictedState(hrp::Vector7 &_x_a_priori, const Eigen::Vector4d &q, const Eigen::Vector3d &gyro, const Eigen::Vector3d &drift) const
Definition: EKFilter.h:39
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
void calcOmega(Eigen::Matrix4d &omega, const Eigen::Vector3d &w) const
Definition: EKFilter.h:30
void calcF(hrp::Matrix77 &F, const Eigen::Vector4d &q, const Eigen::Vector3d &gyro, const Eigen::Vector3d &drift) const
Definition: EKFilter.h:53
void calcRWithFuzzyRule(Eigen::Matrix3d &fuzzyR, const hrp::Vector3 &acc, const hrp::Vector3 &gyro) const
Definition: EKFilter.h:147
Eigen::Vector3d calcMeasurementResidual(const Eigen::Vector3d &acc_measured, const Eigen::Vector4d &q) const
Definition: EKFilter.h:101
EKFilter()
Definition: EKFilter.h:16


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20