forked from Archive/PX4-Autopilot
Whitespace cleanup.
This commit is contained in:
mcsauder
Daniel Agar
parent
4cf8eb8226
commit
21163d859e
|
|
@ -7,4 +7,3 @@
|
|||
|
||||
param set-default FW_THR_CRUISE 0.0
|
||||
param set-default RWTO_TKOFF 0
|
||||
|
||||
|
|
|
|||
|
|
@ -56,4 +56,3 @@ param set-default PWM_MAIN_FUNC1 101
|
|||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
|
||||
|
|
|
|||
|
|
@ -77,4 +77,3 @@ param set-default IMU_DGYRO_CUTOFF 100
|
|||
|
||||
# enable to use high-rate logging for better rate tracking analysis
|
||||
param set-default SDLOG_PROFILE 27
|
||||
|
||||
|
|
|
|||
|
|
@ -17,4 +17,3 @@ def save_compressed(filename):
|
|||
f.write(content_file.read())
|
||||
|
||||
save_compressed(filename)
|
||||
|
||||
|
|
|
|||
|
|
@ -342,4 +342,3 @@ for yaml_file in args.config_files:
|
|||
if verbose: print("Generating {:}".format(params_output_file))
|
||||
with open(params_output_file, 'w') as fid:
|
||||
fid.write(all_params)
|
||||
|
||||
|
|
|
|||
|
|
@ -199,4 +199,3 @@ if __name__ == '__main__':
|
|||
output_groups, timer_params = get_output_groups(timer_groups, verbose=verbose)
|
||||
print('output groups: {:}'.format(output_groups))
|
||||
print('timer params: {:}'.format(timer_params))
|
||||
|
||||
|
|
|
|||
|
|
@ -55,4 +55,3 @@ class JsonOutput():
|
|||
if need_to_write:
|
||||
with codecs.open(filename, 'w', 'utf-8') as f:
|
||||
f.write(json.dumps(self.json,indent=2))
|
||||
|
||||
|
|
|
|||
|
|
@ -2,4 +2,3 @@
|
|||
#
|
||||
# ModalAI FC-v2 specific board defaults
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
|
|
|
|||
|
|
@ -55,4 +55,3 @@ function(px4_list_make_absolute var prefix)
|
|||
endforeach(f)
|
||||
set(${var} "${list_var}" PARENT_SCOPE)
|
||||
endfunction()
|
||||
|
||||
|
|
|
|||
|
|
@ -6,4 +6,3 @@ uint8 NUM_CONTROLS = 8
|
|||
float32[8] control # range: [-1, 1], where 1 means maximum positive thrust,
|
||||
# -1 maximum negative (if not supported by the output, <0 maps to NaN),
|
||||
# and NaN maps to disarmed (stop the motors)
|
||||
|
||||
|
|
|
|||
|
|
@ -6,4 +6,3 @@ uint8 NUM_CONTROLS = 8
|
|||
float32[8] control # range: [-1, 1], where 1 means maximum positive position,
|
||||
# -1 maximum negative,
|
||||
# and NaN maps to disarmed
|
||||
|
||||
|
|
|
|||
|
|
@ -78,7 +78,6 @@ if __name__ == "__main__":
|
|||
```
|
||||
""" % (msg_name, msg_description, msg_url, msg_contents)
|
||||
|
||||
|
||||
with open(output_file, 'w') as content_file:
|
||||
content_file.write(markdown_output)
|
||||
|
||||
|
|
|
|||
|
|
@ -32,4 +32,3 @@
|
|||
############################################################################
|
||||
|
||||
add_subdirectory(${PX4_CHIP})
|
||||
|
||||
|
|
|
|||
|
|
@ -41,4 +41,3 @@ add_subdirectory(lps33hw)
|
|||
add_subdirectory(maiertek)
|
||||
add_subdirectory(ms5611)
|
||||
#add_subdirectory(tcbp001ta) # only for users who really need this
|
||||
|
||||
|
|
|
|||
|
|
@ -9,4 +9,3 @@ actuator_output:
|
|||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
num_channels: 8
|
||||
|
||||
|
|
|
|||
|
|
@ -9,4 +9,3 @@ actuator_output:
|
|||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
num_channels: 16
|
||||
|
||||
|
|
|
|||
|
|
@ -30,4 +30,3 @@ actuator_output:
|
|||
200: PWM200
|
||||
400: PWM400
|
||||
reboot_required: true
|
||||
|
||||
|
|
|
|||
|
|
@ -5,4 +5,3 @@ actuator_output:
|
|||
- param_prefix: ${PWM_MAIN_OR_HIL}
|
||||
channel_label: ${PWM_MAIN_OR_HIL}
|
||||
num_channels: 16
|
||||
|
||||
|
|
|
|||
|
|
@ -26,4 +26,3 @@ actuator_output:
|
|||
200: PWM200
|
||||
400: PWM400
|
||||
reboot_required: true
|
||||
|
||||
|
|
|
|||
|
|
@ -50,4 +50,3 @@ px4_add_module(
|
|||
output_limit
|
||||
tunes
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -4,4 +4,3 @@ actuator_output:
|
|||
- param_prefix: TAP_ESC
|
||||
channel_label: 'TAP ESC'
|
||||
num_channels: 8
|
||||
|
||||
|
|
|
|||
|
|
@ -17,4 +17,3 @@ actuator_output:
|
|||
max: { min: 0, max: 1000, default: 1000 }
|
||||
failsafe: { min: 0, max: 1000 }
|
||||
num_channels: 8
|
||||
|
||||
|
|
|
|||
|
|
@ -89,6 +89,3 @@ add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz
|
|||
COMMENT "Generating component_general.json and checksums.h"
|
||||
)
|
||||
add_custom_target(component_general_json DEPENDS ${component_general_json})
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -110,4 +110,3 @@ add_custom_command(OUTPUT ${generated_events_header}
|
|||
)
|
||||
add_custom_target(events_header DEPENDS ${generated_events_header})
|
||||
add_dependencies(prebuild_targets events_header)
|
||||
|
||||
|
|
|
|||
|
|
@ -240,4 +240,3 @@ private:
|
|||
uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
|
||||
float _data[3] { NAN, NAN, NAN };
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -72,4 +72,3 @@ enum class OutputFunction : int32_t {{
|
|||
{2}
|
||||
}};
|
||||
'''.format(os.path.basename(__file__), yaml_file, function_defs))
|
||||
|
||||
|
|
|
|||
|
|
@ -281,4 +281,3 @@ PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0);
|
|||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_CTRL_ALLOC, 0);
|
||||
|
||||
|
|
|
|||
|
|
@ -19,5 +19,3 @@ quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
|
|||
quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
|
||||
|
||||
return;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -33,4 +33,3 @@ Sigma_wind = param.alignment.windErrNE * [1;1];
|
|||
% Convert to variances and write to covariance matrix diagonals
|
||||
covariance = diag([Sigma_quat;Sigma_velocity;Sigma_position;Sigma_dAngBias;Sigma_dVelBias;Sigma_magNED;Sigma_magXYZ;Sigma_wind].^2);
|
||||
end
|
||||
|
||||
|
|
|
|||
|
|
@ -473,6 +473,7 @@ if isfield(output.innovations,'flowInnov')
|
|||
saveas(h,fullFileName);
|
||||
|
||||
end
|
||||
|
||||
%% plot ZED camera innovations
|
||||
if isfield(output.innovations,'bodyVelInnov')
|
||||
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@ function P = PredictCovariance(deltaAngle, ...
|
|||
states,...
|
||||
P, ... % Previous state covariance matrix
|
||||
dt, ... % IMU and prediction time step
|
||||
param) % tuning parameters
|
||||
param) % tuning parameters
|
||||
|
||||
% Set filter state process noise other than IMU errors, which are already
|
||||
% built into the derived covariance prediction equations.
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ else
|
|||
plotData = sqrt(interp1(plotTimeFull,plotDataFull,plotTime));
|
||||
centerLineData = interp1(centerLineTime,centerLineData,plotTime);
|
||||
|
||||
% plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0);
|
||||
% plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0);
|
||||
if strcmp('IV',varNames{k}(end-1:end))
|
||||
plotDataL = -plotData;
|
||||
plotDataU = plotData;
|
||||
|
|
|
|||
|
|
@ -5,4 +5,3 @@ Created on Sat Mar 14 13:02:26 2020
|
|||
|
||||
@author: roman
|
||||
"""
|
||||
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ int main()
|
|||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
|
||||
|
||||
float Hfusion[24];
|
||||
float Hfusion[24];
|
||||
Vector24f H_MAG;
|
||||
Vector24f Kfusion;
|
||||
float mag_innov_var;
|
||||
|
|
@ -30,11 +30,11 @@ int main()
|
|||
q2 /= length;
|
||||
q3 /= length;
|
||||
|
||||
const float magN = 2.0f * ((float)rand() - 0.5f);
|
||||
const float magE = 2.0f * ((float)rand() - 0.5f);
|
||||
const float magD = 2.0f * ((float)rand() - 0.5f);
|
||||
const float magN = 2.0f * ((float)rand() - 0.5f);
|
||||
const float magE = 2.0f * ((float)rand() - 0.5f);
|
||||
const float magD = 2.0f * ((float)rand() - 0.5f);
|
||||
|
||||
const float R_MAG = sq(0.05f);
|
||||
const float R_MAG = sq(0.05f);
|
||||
const bool update_all_states = true;
|
||||
|
||||
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
|
||||
|
|
@ -50,8 +50,8 @@ int main()
|
|||
}
|
||||
|
||||
// common expressions used by sympy generated equations
|
||||
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss
|
||||
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
|
||||
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss
|
||||
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
|
||||
const float HKX1 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKX2 = magE*q1;
|
||||
const float HKX3 = magD*q0;
|
||||
|
|
@ -78,16 +78,16 @@ int main()
|
|||
const float HKX24 = 1.0F/(HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG);
|
||||
|
||||
// common expressions used by matlab generated equations
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
|
||||
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
|
||||
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2.0f*magN*q0;
|
||||
SH_MAG[8] = 2.0f*magE*q3;
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
|
||||
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
|
||||
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2.0f*magN*q0;
|
||||
SH_MAG[8] = 2.0f*magE*q3;
|
||||
|
||||
// Compare X axis equations
|
||||
{
|
||||
|
|
@ -139,8 +139,8 @@ int main()
|
|||
}
|
||||
|
||||
// repeat calculation using matlab generated equations
|
||||
// X axis innovation variance
|
||||
mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
// X axis innovation variance
|
||||
mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
|
||||
// Calculate X axis observation jacobians
|
||||
H_MAG.setZero();
|
||||
|
|
|
|||
|
|
@ -340,5 +340,3 @@ Kfusion(20) = HKZ70*(-HKZ13*P(16,20) - HKZ15*P(3,20) + HKZ17*P(0,20) + HKZ19*P(1
|
|||
Kfusion(21) = HKZ69*HKZ70;
|
||||
Kfusion(22) = HKZ70*(-HKZ13*P(16,22) - HKZ15*P(3,22) + HKZ17*P(0,22) + HKZ19*P(17,22) + HKZ21*P(18,22) - HKZ23*P(2,22) + HKZ25*P(1,22) - P(21,22));
|
||||
Kfusion(23) = HKZ70*(-HKZ13*P(16,23) - HKZ15*P(3,23) + HKZ17*P(0,23) + HKZ19*P(17,23) + HKZ21*P(18,23) - HKZ23*P(2,23) + HKZ25*P(1,23) - P(21,23));
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -45,5 +45,3 @@ Hfusion.at<2>() = IV18*P(16,21) + IV18*(IV18*P(16,16) + IV19*P(3,16) - IV20*P(0,
|
|||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -191,5 +191,3 @@ Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20)
|
|||
Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) + HK9*P(21,23) - HK9*P(5,21));
|
||||
Kfusion(22) = -HK22*HK32;
|
||||
Kfusion(23) = -HK27*HK32;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -198,5 +198,3 @@ Kfusion(23) = -HK79*HK84;
|
|||
|
||||
|
||||
// Kalman gains - axis 2
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -109,5 +109,3 @@ Kfusion(20) = HK55*(-HK24*P(0,20) - HK34*P(2,20) - HK35*P(3,20) - HK40*P(5,20) -
|
|||
Kfusion(21) = HK55*(-HK24*P(0,21) - HK34*P(2,21) - HK35*P(3,21) - HK40*P(5,21) - HK41*P(6,21) + HK42*P(21,22) - HK43*P(21,23) - HK44*P(4,21) - HK45*P(1,21));
|
||||
Kfusion(22) = HK48*HK55;
|
||||
Kfusion(23) = HK49*HK55;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -524,5 +524,3 @@ nextP(20,23) = P(20,23);
|
|||
nextP(21,23) = P(21,23);
|
||||
nextP(22,23) = P(22,23);
|
||||
nextP(23,23) = P(23,23);
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -226,5 +226,3 @@ Kfusion(20) = -HK66*(HK47*P(0,20) + HK50*P(4,20) + HK52*P(5,20) + HK53*P(6,20) +
|
|||
Kfusion(21) = -HK66*(HK47*P(0,21) + HK50*P(4,21) + HK52*P(5,21) + HK53*P(6,21) + HK54*P(1,21) + HK55*P(2,21) + HK56*P(3,21));
|
||||
Kfusion(22) = -HK66*(HK47*P(0,22) + HK50*P(4,22) + HK52*P(5,22) + HK53*P(6,22) + HK54*P(1,22) + HK55*P(2,22) + HK56*P(3,22));
|
||||
Kfusion(23) = -HK66*(HK47*P(0,23) + HK50*P(4,23) + HK52*P(5,23) + HK53*P(6,23) + HK54*P(1,23) + HK55*P(2,23) + HK56*P(3,23));
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -218,5 +218,3 @@ Kfusion(20) = -HK110*(HK100*P(2,20) + HK101*P(3,20) + HK92*P(0,20) + HK95*P(4,20
|
|||
Kfusion(21) = -HK110*(HK100*P(2,21) + HK101*P(3,21) + HK92*P(0,21) + HK95*P(4,21) + HK97*P(5,21) + HK98*P(6,21) + HK99*P(1,21));
|
||||
Kfusion(22) = -HK110*(HK100*P(2,22) + HK101*P(3,22) + HK92*P(0,22) + HK95*P(4,22) + HK97*P(5,22) + HK98*P(6,22) + HK99*P(1,22));
|
||||
Kfusion(23) = -HK110*(HK100*P(2,23) + HK101*P(3,23) + HK92*P(0,23) + HK95*P(4,23) + HK97*P(5,23) + HK98*P(6,23) + HK99*P(1,23));
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -80,5 +80,3 @@ Kfusion(20) = HK26*(HK12*P(0,20) - HK18*P(1,20) - HK19*P(2,20) + HK20*P(3,20));
|
|||
Kfusion(21) = HK26*(HK12*P(0,21) - HK18*P(1,21) - HK19*P(2,21) + HK20*P(3,21));
|
||||
Kfusion(22) = HK26*(HK12*P(0,22) - HK18*P(1,22) - HK19*P(2,22) + HK20*P(3,22));
|
||||
Kfusion(23) = HK26*(HK12*P(0,23) - HK18*P(1,23) - HK19*P(2,23) + HK20*P(3,23));
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -63,5 +63,3 @@ Kfusion(20) = -HK9*(HK5*P(16,20) - P(17,20));
|
|||
Kfusion(21) = -HK9*(HK5*P(16,21) - P(17,21));
|
||||
Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22));
|
||||
Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23));
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -23,18 +23,18 @@ int main()
|
|||
Vector24f Hfusion_matlab;
|
||||
Vector24f Kfusion_matlab;
|
||||
|
||||
const float R_TAS = sq(1.5f);
|
||||
const float R_TAS = sq(1.5f);
|
||||
|
||||
const bool update_wind_only = false;
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = 9.0f;
|
||||
const float ve = 12.0f;
|
||||
const float vd = -1.5f;
|
||||
// get latest velocity in earth frame
|
||||
const float vn = 9.0f;
|
||||
const float ve = 12.0f;
|
||||
const float vd = -1.5f;
|
||||
|
||||
// get latest wind velocity in earth frame
|
||||
const float vwn = -4.0f;
|
||||
const float vwe = 3.0f;
|
||||
// get latest wind velocity in earth frame
|
||||
const float vwn = -4.0f;
|
||||
const float vwe = 3.0f;
|
||||
|
||||
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
|
||||
SquareMatrix24f P;
|
||||
|
|
@ -77,7 +77,7 @@ int main()
|
|||
const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
|
||||
|
||||
// Observation Jacobians
|
||||
SparseVector24f<4,5,6,22,23> Hfusion;
|
||||
SparseVector24f<4,5,6,22,23> Hfusion;
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK3*vd;
|
||||
|
|
@ -189,11 +189,11 @@ int main()
|
|||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
// find largest Kalman gain difference as a fraction of the matlab value
|
||||
max_diff_fraction = 0.0f;
|
||||
|
|
@ -217,11 +217,11 @@ int main()
|
|||
}
|
||||
}
|
||||
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
if (max_diff_fraction > 1e-5f) {
|
||||
printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
|
||||
} else {
|
||||
printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -70,5 +70,3 @@ Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P
|
|||
Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd);
|
||||
Kfusion(22) = HK15*HK16;
|
||||
Kfusion(23) = HK14*HK16;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -244,5 +244,3 @@ Kfusion(20) = -HK27*(-HK13*P(3,20) - HK14*P(4,20) + HK15*P(0,20) + HK16*P(5,20)
|
|||
Kfusion(21) = -HK27*(-HK13*P(3,21) - HK14*P(4,21) + HK15*P(0,21) + HK16*P(5,21) - HK17*P(2,21) + HK18*P(6,21) + HK19*P(1,21));
|
||||
Kfusion(22) = -HK27*(-HK13*P(3,22) - HK14*P(4,22) + HK15*P(0,22) + HK16*P(5,22) - HK17*P(2,22) + HK18*P(6,22) + HK19*P(1,22));
|
||||
Kfusion(23) = -HK27*(-HK13*P(3,23) - HK14*P(4,23) + HK15*P(0,23) + HK16*P(5,23) - HK17*P(2,23) + HK18*P(6,23) + HK19*P(1,23));
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -247,5 +247,3 @@ Kfusion(20) = -HK85*(-HK71*P(3,20) - HK72*P(4,20) + HK73*P(0,20) + HK74*P(5,20)
|
|||
Kfusion(21) = -HK85*(-HK71*P(3,21) - HK72*P(4,21) + HK73*P(0,21) + HK74*P(5,21) - HK75*P(2,21) + HK76*P(6,21) + HK77*P(1,21));
|
||||
Kfusion(22) = -HK85*(-HK71*P(3,22) - HK72*P(4,22) + HK73*P(0,22) + HK74*P(5,22) - HK75*P(2,22) + HK76*P(6,22) + HK77*P(1,22));
|
||||
Kfusion(23) = -HK85*(-HK71*P(3,23) - HK72*P(4,23) + HK73*P(0,23) + HK74*P(5,23) - HK75*P(2,23) + HK76*P(6,23) + HK77*P(1,23));
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -17,5 +17,3 @@ _ekf_gsf[model_index].P(1,1) = P(1,1) + P(1,2)*S6 + S1*dvyVar + S3*dvxVar + S6*S
|
|||
_ekf_gsf[model_index].P(0,2) = S5;
|
||||
_ekf_gsf[model_index].P(1,2) = S9;
|
||||
_ekf_gsf[model_index].P(2,2) = P(2,2) + dazVar;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -64,5 +64,3 @@ _ekf_gsf[model_index].P(1,1) = P(1,1) - t16*t30 + t22*t26;
|
|||
_ekf_gsf[model_index].P(0,2) = P(0,2) + t19*t24 + t20*t25;
|
||||
_ekf_gsf[model_index].P(1,2) = P(1,2) + t23*t27 + t30*t31;
|
||||
_ekf_gsf[model_index].P(2,2) = P(2,2) - t23*t33 - t25*t36;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -148,5 +148,3 @@ H_YAW.at<20>() = 0;
|
|||
H_YAW.at<21>() = 0;
|
||||
H_YAW.at<22>() = 0;
|
||||
H_YAW.at<23>() = 0;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -95,4 +95,3 @@ H_y_simple = cse(H_y, symbols('t0:30'))
|
|||
|
||||
write_simplified(H_x_simple, "flow_x_observation.txt", "Hx")
|
||||
write_simplified(H_y_simple, "flow_y_observation.txt", "Hy")
|
||||
|
||||
|
|
|
|||
|
|
@ -203,4 +203,3 @@ PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
|
|||
* @value 18 Channel 18
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
|
||||
|
||||
|
|
|
|||
|
|
@ -38,4 +38,3 @@ px4_add_module(
|
|||
sd_stress.c
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue