aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Soaring: fixes for matrixN changes

Browse Source
This commit is contained in:
Andrey Kolobov 2017-03-01 15:12:12 +11:00 committed by Andrew Tridgell
parent 819c70494f
commit 42f5b5a232
3 changed files with 8 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -208,9 +208,9 @@ void SoaringController::init_thermalling()
float cov_q1 = powf(thermal_q1, 2); // State covariance float cov_q1 = powf(thermal_q1, 2); // State covariance
float cov_q2 = powf(thermal_q2, 2); // State covariance float cov_q2 = powf(thermal_q2, 2); // State covariance
const float init_q[4] = {cov_q1, cov_q2, cov_q2, cov_q2}; const float init_q[4] = {cov_q1, cov_q2, cov_q2, cov_q2};
const MatrixN<4> q{init_q}; const MatrixN<float,4> q{init_q};
const float init_p[4] = {INITIAL_STRENGTH_COVARIANCE, INITIAL_RADIUS_COVARIANCE, INITIAL_POSITION_COVARIANCE, INITIAL_POSITION_COVARIANCE}; const float init_p[4] = {INITIAL_STRENGTH_COVARIANCE, INITIAL_RADIUS_COVARIANCE, INITIAL_POSITION_COVARIANCE, INITIAL_POSITION_COVARIANCE};
const MatrixN<4> p{init_p}; const MatrixN<float,4> p{init_p};
// New state vector filter will be reset. Thermal location is placed in front of a/c // New state vector filter will be reset. Thermal location is placed in front of a/c
const float init_xr[4] = {INITIAL_THERMAL_STRENGTH, const float init_xr[4] = {INITIAL_THERMAL_STRENGTH,

7
libraries/AP_Soaring/ExtendedKalmanFilter.cpp
View File

@ -20,7 +20,7 @@ float ExtendedKalmanFilter::measurementpredandjacobian(VectorN<float,N> &A)
} }
void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<N> &p, const MatrixN<N> q, float r) void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<float,N> &p, const MatrixN<float,N> q, float r)
{ {
P = p; P = p;
X = x; X = x;
@ -31,8 +31,7 @@ void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<N> &p,
void ExtendedKalmanFilter::update(float z, float Vx, float Vy) void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
{ {
MatrixN<N> tempM; MatrixN<float,N> tempM;
VectorN<float,N> tempV;
VectorN<float,N> H; VectorN<float,N> H;
VectorN<float,N> P12; VectorN<float,N> P12;
VectorN<float,N> K; VectorN<float,N> K;
@ -57,7 +56,7 @@ void ExtendedKalmanFilter::update(float z, float Vx, float Vy)
// LINE 40 // LINE 40
// P12 = P * H'; // P12 = P * H';
P.mult(H, P12); //cross covariance P12.mult(P, H); //cross covariance
// LINE 41 // LINE 41
// Calculate the KALMAN GAIN // Calculate the KALMAN GAIN

6
libraries/AP_Soaring/ExtendedKalmanFilter.h
View File

@ -16,10 +16,10 @@ public:
ExtendedKalmanFilter(void) {} ExtendedKalmanFilter(void) {}
VectorN<float,N> X; VectorN<float,N> X;
MatrixN<N> P; MatrixN<float,N> P;
MatrixN<N> Q; MatrixN<float,N> Q;
float R; float R;
void reset(const VectorN<float,N> &x, const MatrixN<N> &p, const MatrixN<N> q, float r); void reset(const VectorN<float,N> &x, const MatrixN<float,N> &p, const MatrixN<float,N> q, float r);
void update(float z, float Vx, float Vy); void update(float z, float Vx, float Vy);
private: private: