fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 13:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: save some stack memory

Browse Source
This commit is contained in:
Paul Riseborough
2016-11-02 09:06:06 +01:00
committed by Lorenz Meier
parent f472324ae8
commit 1b59a89a18
4 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+3
View File
@@ -309,6 +309,7 @@ void Ekf::fuseMag()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column <= 3; column++) {
KH[row][column] = Kfusion[row] * H_MAG[column];
@@ -638,6 +639,7 @@ void Ekf::fuseHeading()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column <= 3; column++) {
KH[row][column] = Kfusion[row] * H_YAW[column];
@@ -775,6 +777,7 @@ void Ekf::fuseDeclination()
// first calculate expression for KHP
// then calculate P - KHP
// take advantage of the empty columns in KH to reduce the number of operations
float KH[_k_num_states][_k_num_states];
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 16; column <= 17; column++) {
KH[row][column] = Kfusion[row] * H_DECL[column];