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

removed reinitialization of intermediate variables

Browse Source
This commit is contained in:
CarlOlsson
2016-04-05 14:18:47 +02:00
committed by Roman Bapst
parent 2632c930f7
commit 8678a939e2
1 changed files with 1 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+1 -5
View File
@@ -148,7 +148,7 @@ void Ekf::fuseAirspeed()
_state.ang_error.setZero();
// Fuse airspeed measurement
fuse(Kfusion, _airspeed_innov); //Why calculate angle error when it is always zero?
fuse(Kfusion, _airspeed_innov);
// correct the nominal quaternion
Quaternion dq;
@@ -156,10 +156,6 @@ void Ekf::fuseAirspeed()
_state.quat_nominal = dq * _state.quat_nominal;
_state.quat_nominal.normalize();
// update covariance matrix via Pnew = (I - KH)P = P - KHP
float KH[_k_num_states][_k_num_states] = {};
float KHP[_k_num_states][_k_num_states] = {};
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that...
KH[row][column] = Kfusion[row] * H_TAS[column];