forked from Archive/PX4-Autopilot
Whitespace cleanup.
This commit is contained in:
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))
|
||||
|
||||
|
|
|
@ -25,7 +25,7 @@ div.frame_common table, div.frame_common table {
|
|||
}
|
||||
|
||||
div.frame_common table {
|
||||
float: right;
|
||||
float: right;
|
||||
width: 70%;
|
||||
}
|
||||
|
||||
|
|
|
@ -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))
|
||||
|
||||
|
|
|
@ -1,5 +1,5 @@
|
|||
//Public key to verify signed binaries
|
||||
0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c,
|
||||
0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81,
|
||||
0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb,
|
||||
0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd,
|
||||
0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c,
|
||||
0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81,
|
||||
0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb,
|
||||
0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd,
|
||||
|
|
|
@ -1,38 +1,38 @@
|
|||
|
||||
0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9,
|
||||
0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1,
|
||||
0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0,
|
||||
0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1,
|
||||
0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37,
|
||||
0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a,
|
||||
0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e,
|
||||
0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83,
|
||||
0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f,
|
||||
0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c,
|
||||
0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc,
|
||||
0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6,
|
||||
0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95,
|
||||
0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf,
|
||||
0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77,
|
||||
0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7,
|
||||
0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60,
|
||||
0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27,
|
||||
0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd,
|
||||
0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3,
|
||||
0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69,
|
||||
0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80,
|
||||
0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a,
|
||||
0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6,
|
||||
0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0,
|
||||
0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b,
|
||||
0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1,
|
||||
0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2,
|
||||
0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98,
|
||||
0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19,
|
||||
0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a,
|
||||
0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1,
|
||||
0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e,
|
||||
0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f,
|
||||
0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70,
|
||||
0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0,
|
||||
0xcb, 0x2, 0x3, 0x1, 0x0, 0x1,
|
||||
0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9,
|
||||
0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1,
|
||||
0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0,
|
||||
0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1,
|
||||
0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37,
|
||||
0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a,
|
||||
0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e,
|
||||
0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83,
|
||||
0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f,
|
||||
0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c,
|
||||
0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc,
|
||||
0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6,
|
||||
0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95,
|
||||
0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf,
|
||||
0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77,
|
||||
0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7,
|
||||
0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60,
|
||||
0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27,
|
||||
0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd,
|
||||
0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3,
|
||||
0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69,
|
||||
0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80,
|
||||
0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a,
|
||||
0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6,
|
||||
0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0,
|
||||
0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b,
|
||||
0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1,
|
||||
0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2,
|
||||
0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98,
|
||||
0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19,
|
||||
0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a,
|
||||
0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1,
|
||||
0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e,
|
||||
0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f,
|
||||
0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70,
|
||||
0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0,
|
||||
0xcb, 0x2, 0x3, 0x1, 0x0, 0x1,
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -28,24 +28,24 @@ if __name__ == "__main__":
|
|||
msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"..")
|
||||
msg_files = get_msgs_list(msg_path)
|
||||
msg_files.sort()
|
||||
|
||||
|
||||
filelist_in_markdown=''
|
||||
|
||||
|
||||
for msg_file in msg_files:
|
||||
msg_name = os.path.splitext(msg_file)[0]
|
||||
output_file = os.path.join(output_dir, msg_name+'.md')
|
||||
msg_filename = os.path.join(msg_path, msg_file)
|
||||
print("{:} -> {:}".format(msg_filename, output_file))
|
||||
|
||||
|
||||
#Format msg url
|
||||
msg_url="[source file](https://github.com/PX4/PX4-Autopilot/blob/master/msg/%s)" % msg_file
|
||||
|
||||
|
||||
msg_description = ""
|
||||
summary_description = ""
|
||||
|
||||
#Get msg description (first non-empty comment line from top of msg)
|
||||
with open(msg_filename, 'r') as lineparser:
|
||||
line = lineparser.readline()
|
||||
line = lineparser.readline()
|
||||
while line.startswith('#') or (line.strip() == ''):
|
||||
print('DEBUG: line: %s' % line)
|
||||
line=line[1:].strip()+'\n'
|
||||
|
@ -65,10 +65,10 @@ if __name__ == "__main__":
|
|||
#Get msg contents (read the file)
|
||||
with open(msg_filename, 'r') as source_file:
|
||||
msg_contents = source_file.read()
|
||||
|
||||
|
||||
#Format markdown using msg name, comment, url, contents.
|
||||
markdown_output="""# %s (UORB message)
|
||||
|
||||
|
||||
%s
|
||||
|
||||
%s
|
||||
|
@ -78,27 +78,26 @@ if __name__ == "__main__":
|
|||
```
|
||||
""" % (msg_name, msg_description, msg_url, msg_contents)
|
||||
|
||||
|
||||
with open(output_file, 'w') as content_file:
|
||||
content_file.write(markdown_output)
|
||||
|
||||
|
||||
readme_markdown_file_link='- [%s](%s.md)' % (msg_name,msg_name)
|
||||
if summary_description:
|
||||
readme_markdown_file_link+=" — %s" % summary_description
|
||||
filelist_in_markdown+=readme_markdown_file_link+"\n"
|
||||
|
||||
|
||||
# Write out the README.md file
|
||||
readme_text="""# uORB Message Reference
|
||||
|
||||
:::note
|
||||
This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/msg/tools/generate_msg_docs.py) from the source code.
|
||||
:::
|
||||
|
||||
|
||||
This topic lists the UORB messages available in PX4 (some of which may be may be shared by the [PX4-ROS 2 Bridge](../ros/ros2_comm.md)).
|
||||
Graphs showing how these are used [can be found here](../middleware/uorb_graph.md).
|
||||
|
||||
|
||||
%s
|
||||
""" % (filelist_in_markdown)
|
||||
readme_file = os.path.join(output_dir, 'README.md')
|
||||
with open(readme_file, 'w') as content_file:
|
||||
content_file.write(readme_text)
|
||||
content_file.write(readme_text)
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -4,7 +4,7 @@ actuator_output:
|
|||
- generator: pwm
|
||||
param_prefix: '${PWM_MAIN_OR_AUX}'
|
||||
channel_labels: ['${PWM_MAIN_OR_AUX}', 'PWM Capture']
|
||||
standard_params:
|
||||
standard_params:
|
||||
disarmed: { min: 800, max: 2200, default: 900 }
|
||||
min: { min: 800, max: 1400, default: 1000 }
|
||||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
|
@ -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;
|
||||
|
||||
|
||||
|
|
|
@ -8,7 +8,7 @@ q0 = quat(1);
|
|||
q1 = quat(2);
|
||||
q2 = quat(3);
|
||||
q3 = quat(4);
|
||||
|
||||
|
||||
Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
|
||||
2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
|
||||
2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];
|
||||
2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];
|
||||
|
|
|
@ -1,5 +1,5 @@
|
|||
function quatOut = QuatMult(quatA,quatB)
|
||||
% Calculate the following quaternion product quatA * quatB using the
|
||||
% Calculate the following quaternion product quatA * quatB using the
|
||||
% standard identity
|
||||
|
||||
quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))];
|
|
@ -1,6 +1,6 @@
|
|||
function quat = AlignHeading( ...
|
||||
quat, ... % quaternion state vector
|
||||
magMea, ... % body frame magnetic flux measurements
|
||||
magMea, ... % body frame magnetic flux measurements
|
||||
declination) % Estimated magnetic field delination at current location (rad)
|
||||
|
||||
% Get the Euler angles and set yaw to zero
|
||||
|
@ -20,4 +20,4 @@ euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1));
|
|||
% convert to a quaternion
|
||||
quat = EulToQuat(euler);
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
@ -28,7 +28,7 @@ relVelBodyPred = transpose(Tbn)*[vn;ve;vd];
|
|||
|
||||
% calculate the observation jacobian, innovation variance and innovation
|
||||
for obsIndex = 1:3
|
||||
|
||||
|
||||
% Calculate corrections using X component
|
||||
if (obsIndex == 1)
|
||||
H(1,:) = calcH_VELX(q0,q1,q2,q3,vd,ve,vn);
|
||||
|
@ -53,30 +53,30 @@ end
|
|||
for obsIndex = 1:3
|
||||
|
||||
Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
|
||||
|
||||
|
||||
% correct the state vector
|
||||
states = states - Kfusion * innovation(obsIndex);
|
||||
|
||||
|
||||
% normalise the updated quaternion states
|
||||
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
|
||||
if (quatMag > 1e-12)
|
||||
states(1:4) = states(1:4) / quatMag;
|
||||
end
|
||||
|
||||
|
||||
% correct the covariance P = P - K*H*P
|
||||
P = P - Kfusion*H(obsIndex,:)*P;
|
||||
|
||||
|
||||
% Force symmetry on the covariance matrix to prevent ill-conditioning
|
||||
% of the matrix which would cause the filter to blow-up
|
||||
P = 0.5*(P + transpose(P));
|
||||
|
||||
|
||||
% ensure diagonals are positive
|
||||
for i=1:24
|
||||
if P(i,i) < 0
|
||||
P(i,i) = 0;
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
@ -33,7 +33,7 @@ magPred = transpose(Tbn)*[magN;magE;magD] + [magXbias;magYbias;magZbias];
|
|||
|
||||
% calculate the observation jacobian, innovation variance and innovation
|
||||
for obsIndex = 1:3
|
||||
|
||||
|
||||
% Calculate corrections using X component
|
||||
if (obsIndex == 1)
|
||||
H(1,:) = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3);
|
||||
|
@ -58,30 +58,30 @@ end
|
|||
for obsIndex = 1:3
|
||||
|
||||
Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
|
||||
|
||||
|
||||
% correct the state vector
|
||||
states = states - Kfusion * innovation(obsIndex);
|
||||
|
||||
|
||||
% normalise the updated quaternion states
|
||||
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
|
||||
if (quatMag > 1e-12)
|
||||
states(1:4) = states(1:4) / quatMag;
|
||||
end
|
||||
|
||||
|
||||
% correct the covariance P = P - K*H*P
|
||||
P = P - Kfusion*H(obsIndex,:)*P;
|
||||
|
||||
|
||||
% Force symmetry on the covariance matrix to prevent ill-conditioning
|
||||
% of the matrix which would cause the filter to blow-up
|
||||
P = 0.5*(P + transpose(P));
|
||||
|
||||
|
||||
% ensure diagonals are positive
|
||||
for i=1:24
|
||||
if P(i,i) < 0
|
||||
P(i,i) = 0;
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
@ -36,7 +36,7 @@ losRateMea = - flowRate + bodyRate;
|
|||
|
||||
% calculate the observation jacobian, innovation variance and innovation
|
||||
for obsIndex = 1:2
|
||||
|
||||
|
||||
% Calculate corrections using X component
|
||||
if (obsIndex == 1)
|
||||
H(1,:) = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn);
|
||||
|
@ -59,30 +59,30 @@ end
|
|||
for obsIndex = 1:2
|
||||
|
||||
Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
|
||||
|
||||
|
||||
% correct the state vector
|
||||
states = states - Kfusion * innovation(obsIndex);
|
||||
|
||||
|
||||
% normalise the updated quaternion states
|
||||
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
|
||||
if (quatMag > 1e-12)
|
||||
states(1:4) = states(1:4) / quatMag;
|
||||
end
|
||||
|
||||
|
||||
% correct the covariance P = P - K*H*P
|
||||
P = P - Kfusion*H(obsIndex,:)*P;
|
||||
|
||||
|
||||
% Force symmetry on the covariance matrix to prevent ill-conditioning
|
||||
% of the matrix which would cause the filter to blow-up
|
||||
P = 0.5*(P + transpose(P));
|
||||
|
||||
|
||||
% ensure diagonals are positive
|
||||
for i=1:24
|
||||
if P(i,i) < 0
|
||||
P(i,i) = 0;
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
@ -15,54 +15,54 @@ varInnov = zeros(1,2);
|
|||
H = zeros(2,24);
|
||||
|
||||
for obsIndex = 1:2
|
||||
|
||||
|
||||
% velocity states start at index 8
|
||||
stateIndex = 7 + obsIndex;
|
||||
|
||||
% Calculate the velocity measurement innovation
|
||||
innovation(obsIndex) = states(stateIndex) - measPos(obsIndex);
|
||||
|
||||
|
||||
% Calculate the observation Jacobian
|
||||
H(obsIndex,stateIndex) = 1;
|
||||
|
||||
|
||||
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
|
||||
|
||||
|
||||
end
|
||||
|
||||
% Apply an innovation consistency check
|
||||
for obsIndex = 1:2
|
||||
|
||||
|
||||
if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
|
||||
return;
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
% Calculate Kalman gains and update states and covariances
|
||||
for obsIndex = 1:2
|
||||
|
||||
|
||||
% Calculate the Kalman gains
|
||||
K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
|
||||
|
||||
|
||||
% Calculate state corrections
|
||||
xk = K * innovation(obsIndex);
|
||||
|
||||
|
||||
% Apply the state corrections
|
||||
states = states - xk;
|
||||
|
||||
|
||||
% Update the covariance
|
||||
P = P - K*H(obsIndex,:)*P;
|
||||
|
||||
|
||||
% Force symmetry on the covariance matrix to prevent ill-conditioning
|
||||
P = 0.5*(P + transpose(P));
|
||||
|
||||
|
||||
% ensure diagonals are positive
|
||||
for i=1:24
|
||||
if P(i,i) < 0
|
||||
P(i,i) = 0;
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
@ -15,54 +15,54 @@ varInnov = zeros(1,3);
|
|||
H = zeros(3,24);
|
||||
|
||||
for obsIndex = 1:3
|
||||
|
||||
|
||||
% velocity states start at index 5
|
||||
stateIndex = 4 + obsIndex;
|
||||
|
||||
% Calculate the velocity measurement innovation
|
||||
innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);
|
||||
|
||||
|
||||
% Calculate the observation Jacobian
|
||||
H(obsIndex,stateIndex) = 1;
|
||||
|
||||
|
||||
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
|
||||
|
||||
|
||||
end
|
||||
|
||||
% Apply an innovation consistency check
|
||||
for obsIndex = 1:3
|
||||
|
||||
|
||||
if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
|
||||
return;
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
% Calculate Kalman gains and update states and covariances
|
||||
for obsIndex = 1:3
|
||||
|
||||
|
||||
% Calculate the Kalman gains
|
||||
K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
|
||||
|
||||
|
||||
% Calculate state corrections
|
||||
xk = K * innovation(obsIndex);
|
||||
|
||||
|
||||
% Apply the state corrections
|
||||
states = states - xk;
|
||||
|
||||
|
||||
% Update the covariance
|
||||
P = P - K*H(obsIndex,:)*P;
|
||||
|
||||
|
||||
% Force symmetry on the covariance matrix to prevent ill-conditioning
|
||||
P = 0.5*(P + transpose(P));
|
||||
|
||||
|
||||
% ensure diagonals are positive
|
||||
for i=1:24
|
||||
if P(i,i) < 0
|
||||
P(i,i) = 0;
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
@ -32,7 +32,7 @@ quat = [q0;q1;q2;q3];
|
|||
Tbn = Quat2Tbn(quat);
|
||||
|
||||
% define the truth delta angle
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% covariance growth for our application and grade of sensor
|
||||
dAngTruth = dAngMeas - dAngBias;
|
||||
|
||||
|
@ -137,7 +137,7 @@ f = matlabFunction(H_MAGD,'file','calcH_MAGD.m');
|
|||
%% derive equations for fusion of a single magneic compass heading measurement
|
||||
|
||||
% rotate body measured field into earth axes
|
||||
magMeasNED = Tbn*[magX;magY;magZ];
|
||||
magMeasNED = Tbn*[magX;magY;magZ];
|
||||
|
||||
% the predicted measurement is the angle wrt true north of the horizontal
|
||||
% component of the measured field
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -215,7 +215,7 @@ if (output.magFuseMethod <= 1)
|
|||
set(h,'PaperOrientation','portrait');
|
||||
set(h,'PaperUnits','normalized');
|
||||
set(h,'PaperPosition', [0 0 1 1]);
|
||||
|
||||
|
||||
subplot(3,1,1);
|
||||
plot(output.time_lapsed',[output.mag_XYZ(:,1),output.mag_XYZ(:,1)+2*sqrt(output.state_variances(:,20)),output.mag_XYZ(:,1)-2*sqrt(output.state_variances(:,20))]);
|
||||
grid on;
|
||||
|
@ -224,21 +224,21 @@ if (output.magFuseMethod <= 1)
|
|||
ylabel('X bias (gauss)');
|
||||
xlabel('time (sec)');
|
||||
legend('estimate','upper 95% bound','lower 95% bound');
|
||||
|
||||
|
||||
subplot(3,1,2);
|
||||
plot(output.time_lapsed',[output.mag_XYZ(:,2),output.mag_XYZ(:,2)+2*sqrt(output.state_variances(:,21)),output.mag_XYZ(:,2)-2*sqrt(output.state_variances(:,21))]);
|
||||
grid on;
|
||||
ylabel('Y bias (gauss)');
|
||||
xlabel('time (sec)');
|
||||
legend('estimate','upper 95% bound','lower 95% bound');
|
||||
|
||||
|
||||
subplot(3,1,3);
|
||||
plot(output.time_lapsed',[output.mag_XYZ(:,3),output.mag_XYZ(:,3)+2*sqrt(output.state_variances(:,22)),output.mag_XYZ(:,3)-2*sqrt(output.state_variances(:,22))]);
|
||||
grid on;
|
||||
ylabel('Z bias (gauss)');
|
||||
xlabel('time (sec)');
|
||||
legend('estimate','upper 95% bound','lower 95% bound');
|
||||
|
||||
|
||||
fileName='body_field_estimates.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
@ -251,9 +251,9 @@ if (output.magFuseMethod <= 1)
|
|||
set(h,'PaperOrientation','portrait');
|
||||
set(h,'PaperUnits','normalized');
|
||||
set(h,'PaperPosition', [0 0 1 1]);
|
||||
|
||||
|
||||
margin = 0.1;
|
||||
|
||||
|
||||
subplot(4,1,1);
|
||||
plot(output.time_lapsed',[output.mag_NED(:,1),output.mag_NED(:,1)+2*sqrt(output.state_variances(:,17)),output.mag_NED(:,1)-2*sqrt(output.state_variances(:,17))]);
|
||||
minVal = min(output.mag_NED(:,1))-margin;
|
||||
|
@ -265,7 +265,7 @@ if (output.magFuseMethod <= 1)
|
|||
ylabel('North (gauss)');
|
||||
xlabel('time (sec)');
|
||||
legend('estimate','upper 95% bound','lower 95% bound');
|
||||
|
||||
|
||||
subplot(4,1,2);
|
||||
plot(output.time_lapsed',[output.mag_NED(:,2),output.mag_NED(:,2)+2*sqrt(output.state_variances(:,18)),output.mag_NED(:,2)-2*sqrt(output.state_variances(:,18))]);
|
||||
minVal = min(output.mag_NED(:,2))-margin;
|
||||
|
@ -275,7 +275,7 @@ if (output.magFuseMethod <= 1)
|
|||
ylabel('East (gauss)');
|
||||
xlabel('time (sec)');
|
||||
legend('estimate','upper 95% bound','lower 95% bound');
|
||||
|
||||
|
||||
subplot(4,1,3);
|
||||
plot(output.time_lapsed',[output.mag_NED(:,3),output.mag_NED(:,3)+2*sqrt(output.state_variances(:,19)),output.mag_NED(:,3)-2*sqrt(output.state_variances(:,19))]);
|
||||
grid on;
|
||||
|
@ -285,7 +285,7 @@ if (output.magFuseMethod <= 1)
|
|||
ylabel('Down (gauss)');
|
||||
xlabel('time (sec)');
|
||||
legend('estimate','upper 95% bound','lower 95% bound');
|
||||
|
||||
|
||||
subplot(4,1,4);
|
||||
plot(output.time_lapsed',rad2deg*atan2(output.mag_NED(:,2),output.mag_NED(:,1)));
|
||||
grid on;
|
||||
|
@ -293,7 +293,7 @@ if (output.magFuseMethod <= 1)
|
|||
title(titleText);
|
||||
ylabel('declination (deg)');
|
||||
xlabel('time (sec)');
|
||||
|
||||
|
||||
fileName='earth_field_estimates.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
@ -301,13 +301,13 @@ end
|
|||
|
||||
%% plot velocity innovations
|
||||
if isfield(output.innovations,'vel_innov')
|
||||
|
||||
|
||||
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
|
||||
h=gcf;
|
||||
set(h,'PaperOrientation','portrait');
|
||||
set(h,'PaperUnits','normalized');
|
||||
set(h,'PaperPosition', [0 0 1 1]);
|
||||
|
||||
|
||||
subplot(3,1,1);
|
||||
plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,1),sqrt(output.innovations.vel_innov_var(:,1)),-sqrt(output.innovations.vel_innov_var(:,1))]);
|
||||
grid on;
|
||||
|
@ -316,21 +316,21 @@ if isfield(output.innovations,'vel_innov')
|
|||
ylabel('North (m/s)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','variance sqrt','variance sqrt');
|
||||
|
||||
|
||||
subplot(3,1,2);
|
||||
plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,2),sqrt(output.innovations.vel_innov_var(:,2)),-sqrt(output.innovations.vel_innov_var(:,2))]);
|
||||
grid on;
|
||||
ylabel('East (m/s)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','variance sqrt','variance sqrt');
|
||||
|
||||
|
||||
subplot(3,1,3);
|
||||
plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,3),sqrt(output.innovations.vel_innov_var(:,3)),-sqrt(output.innovations.vel_innov_var(:,3))]);
|
||||
grid on;
|
||||
ylabel('Down (m/s)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','variance sqrt','variance sqrt');
|
||||
|
||||
|
||||
fileName='velocity_fusion.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
@ -343,7 +343,7 @@ if isfield(output.innovations,'posInnov')
|
|||
set(h,'PaperOrientation','portrait');
|
||||
set(h,'PaperUnits','normalized');
|
||||
set(h,'PaperPosition', [0 0 1 1]);
|
||||
|
||||
|
||||
subplot(3,1,1);
|
||||
plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,1),sqrt(output.innovations.posInnovVar(:,1)),-sqrt(output.innovations.posInnovVar(:,1))]);
|
||||
grid on;
|
||||
|
@ -352,21 +352,21 @@ if isfield(output.innovations,'posInnov')
|
|||
ylabel('North (m)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','variance sqrt','variance sqrt');
|
||||
|
||||
|
||||
subplot(3,1,2);
|
||||
plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,2),sqrt(output.innovations.posInnovVar(:,2)),-sqrt(output.innovations.posInnovVar(:,2))]);
|
||||
grid on;
|
||||
ylabel('East (m)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','variance sqrt','variance sqrt');
|
||||
|
||||
|
||||
subplot(3,1,3);
|
||||
plot(output.innovations.hgt_time_lapsed',[output.innovations.hgtInnov(:),sqrt(output.innovations.hgtInnovVar(:)),-sqrt(output.innovations.hgtInnovVar(:))]);
|
||||
grid on;
|
||||
ylabel('Up (m)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','variance sqrt','variance sqrt');
|
||||
|
||||
|
||||
fileName='position_fusion.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
@ -374,7 +374,7 @@ end
|
|||
|
||||
%% plot magnetometer innovations
|
||||
if isfield(output.innovations,'magInnov')
|
||||
|
||||
|
||||
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
|
||||
h=gcf;
|
||||
set(h,'PaperOrientation','portrait');
|
||||
|
@ -412,18 +412,18 @@ if isfield(output.innovations,'magInnov')
|
|||
fileName='magnetometer_fusion.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
||||
|
||||
end
|
||||
|
||||
%% plot magnetic yaw innovations
|
||||
if isfield(output.innovations,'hdgInnov')
|
||||
|
||||
|
||||
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
|
||||
h=gcf;
|
||||
set(h,'PaperOrientation','portrait');
|
||||
set(h,'PaperUnits','normalized');
|
||||
set(h,'PaperPosition', [0 0 1 1]);
|
||||
|
||||
|
||||
subplot(2,1,1);
|
||||
plot(output.innovations.mag_time_lapsed,[output.innovations.hdgInnov*rad2deg;sqrt(output.innovations.hdgInnovVar)*rad2deg;-sqrt(output.innovations.hdgInnovVar)*rad2deg]);
|
||||
ylim([-30 30]);
|
||||
|
@ -442,12 +442,12 @@ if isfield(output.innovations,'hdgInnov')
|
|||
fileName='magnetometer_fusion.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
||||
|
||||
end
|
||||
|
||||
%% plot optical flow innovations
|
||||
if isfield(output.innovations,'flowInnov')
|
||||
|
||||
|
||||
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
|
||||
h=gcf;
|
||||
set(h,'PaperOrientation','portrait');
|
||||
|
@ -471,17 +471,18 @@ if isfield(output.innovations,'flowInnov')
|
|||
fileName='optical_flow_fusion.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
||||
|
||||
end
|
||||
|
||||
%% plot ZED camera innovations
|
||||
if isfield(output.innovations,'bodyVelInnov')
|
||||
|
||||
|
||||
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
|
||||
h=gcf;
|
||||
set(h,'PaperOrientation','portrait');
|
||||
set(h,'PaperUnits','normalized');
|
||||
set(h,'PaperPosition', [0 0 1 1]);
|
||||
|
||||
|
||||
subplot(3,1,1);
|
||||
plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,1)';sqrt(output.innovations.bodyVelInnovVar(:,1))';-sqrt(output.innovations.bodyVelInnovVar(:,1))']);
|
||||
grid on;
|
||||
|
@ -489,23 +490,23 @@ if isfield(output.innovations,'bodyVelInnov')
|
|||
ylabel('X (m/sec)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','innovation variance sqrt','innovation variance sqrt');
|
||||
|
||||
|
||||
subplot(3,1,2);
|
||||
plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,2)';sqrt(output.innovations.bodyVelInnovVar(:,2))';-sqrt(output.innovations.bodyVelInnovVar(:,2))']);
|
||||
grid on;
|
||||
ylabel('Y (m/sec)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','innovation variance sqrt','innovation variance sqrt');
|
||||
|
||||
|
||||
subplot(3,1,3);
|
||||
plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,3)';sqrt(output.innovations.bodyVelInnovVar(:,3))';-sqrt(output.innovations.bodyVelInnovVar(:,3))']);
|
||||
grid on;
|
||||
ylabel('Z (m/sec)');
|
||||
xlabel('time (sec)');
|
||||
legend('innovation','innovation variance sqrt','innovation variance sqrt');
|
||||
|
||||
|
||||
fileName='zed_camera_fusion.png';
|
||||
fullFileName = fullfile(folder, fileName);
|
||||
saveas(h,fullFileName);
|
||||
|
||||
|
||||
end
|
||||
|
|
|
@ -3,8 +3,8 @@ 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.
|
||||
% This process noise determines the rate of estimation of IMU bias errors
|
||||
|
@ -63,4 +63,4 @@ for i=1:24
|
|||
end
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
@ -44,10 +44,10 @@ delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt;
|
|||
|
||||
% Apply corrections for coning errors and earth spin rate
|
||||
% Coning correction from :
|
||||
% "A new strapdown attitude algorithm",
|
||||
% R. B. MILLER,
|
||||
% "A new strapdown attitude algorithm",
|
||||
% R. B. MILLER,
|
||||
% Journal of Guidance, Control, and Dynamics
|
||||
% July, Vol. 6, No. 4, pp. 287-291, Eqn 11
|
||||
% July, Vol. 6, No. 4, pp. 287-291, Eqn 11
|
||||
% correctedDelAng = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED;
|
||||
correctedDelAng = delAng - transpose(Tbn_prev)*delAngEarth_NED;
|
||||
|
||||
|
|
|
@ -106,20 +106,20 @@ last_synthetic_velocity_fusion_time = 0;
|
|||
last_valid_range_time = - param.fusion.rngTimeout;
|
||||
|
||||
for index = indexStart:indexStop
|
||||
|
||||
|
||||
% read IMU measurements
|
||||
local_time=imu_data.time_us(imuIndex)*1e-6;
|
||||
delta_angle(:,1) = imu_data.del_ang(imuIndex,:);
|
||||
delta_velocity(:,1) = imu_data.del_vel(imuIndex,:);
|
||||
dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex));
|
||||
imuIndex = imuIndex+1;
|
||||
|
||||
|
||||
% predict states
|
||||
[states, delAngCorrected, delVelCorrected] = PredictStates(states,delta_angle,delta_velocity,imu_data.accel_dt(imuIndex),gravity,gps_data.refLLH(1,1)*deg2rad);
|
||||
|
||||
|
||||
% constrain states
|
||||
[states] = ConstrainStates(states,dt_imu_avg);
|
||||
|
||||
|
||||
dtCov = dtCov + dt_imu;
|
||||
delAngCov = delAngCov + delAngCorrected;
|
||||
delVelCov = delVelCov + delVelCorrected;
|
||||
|
@ -131,7 +131,7 @@ for index = indexStart:indexStop
|
|||
dtCovInt = dtCovInt + dtCov;
|
||||
dtCov = 0;
|
||||
covIndex = covIndex + 1;
|
||||
|
||||
|
||||
% output state data
|
||||
output.time_lapsed(covIndex) = local_time;
|
||||
output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')';
|
||||
|
@ -142,22 +142,22 @@ for index = indexStart:indexStop
|
|||
output.mag_NED(covIndex,:) = states(17:19);
|
||||
output.mag_XYZ(covIndex,:) = states(20:22);
|
||||
output.wind_NE(covIndex,:) = states(23:24);
|
||||
|
||||
|
||||
% output covariance data
|
||||
for i=1:24
|
||||
output.state_variances(covIndex,i) = covariance(i,i);
|
||||
end
|
||||
|
||||
|
||||
% output equivalent euler angle variances
|
||||
error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4));
|
||||
euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix);
|
||||
for i=1:3
|
||||
output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i);
|
||||
end
|
||||
|
||||
|
||||
% Get most recent GPS data that had fallen behind the fusion time horizon
|
||||
latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
|
||||
|
||||
if ~isempty(latest_gps_index)
|
||||
% Check if GPS use is being blocked by the user
|
||||
if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime))
|
||||
|
@ -166,7 +166,7 @@ for index = indexStart:indexStop
|
|||
else
|
||||
gps_use_blocked = false;
|
||||
end
|
||||
|
||||
|
||||
% If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS
|
||||
if (~gps_use_started && ~gps_use_blocked)
|
||||
if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim))
|
||||
|
@ -176,24 +176,24 @@ for index = indexStart:indexStop
|
|||
last_drift_constrain_time = local_time;
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
% Fuse GPS data when available if GPS use has started
|
||||
if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index))
|
||||
last_gps_index = latest_gps_index;
|
||||
gps_fuse_index = gps_fuse_index + 1;
|
||||
last_drift_constrain_time = local_time;
|
||||
|
||||
|
||||
% fuse NED GPS velocity
|
||||
[states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index));
|
||||
|
||||
|
||||
% data logging
|
||||
output.innovations.vel_time_lapsed(gps_fuse_index) = local_time;
|
||||
output.innovations.vel_innov(gps_fuse_index,:) = velInnov';
|
||||
output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
|
||||
|
||||
|
||||
% fuse NE GPS position
|
||||
[states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index));
|
||||
|
||||
|
||||
% data logging
|
||||
output.innovations.pos_time_lapsed(gps_fuse_index) = local_time;
|
||||
output.innovations.posInnov(gps_fuse_index,:) = posInnov';
|
||||
|
@ -208,13 +208,13 @@ for index = indexStart:indexStop
|
|||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
% Fuse new Baro data that has fallen beind the fusion time horizon
|
||||
latest_baro_index = find((baro_data.time_us - 1e6 * param.fusion.baroTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_baro_index > last_baro_index)
|
||||
last_baro_index = latest_baro_index;
|
||||
baro_fuse_index = baro_fuse_index + 1;
|
||||
|
||||
|
||||
% fuse baro height
|
||||
[states,covariance,hgtInnov,hgtInnovVar] = FuseBaroHeight(states,covariance,baro_data.height(latest_baro_index),param.fusion.baroHgtGate,param.fusion.baroHgtNoise);
|
||||
|
||||
|
@ -223,125 +223,125 @@ for index = indexStart:indexStop
|
|||
output.innovations.hgtInnov(baro_fuse_index) = hgtInnov;
|
||||
output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar;
|
||||
end
|
||||
|
||||
|
||||
% Fuse new mag data that has fallen behind the fusion time horizon
|
||||
latest_mag_index = find((mag_data.time_us - 1e6 * param.fusion.magTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_mag_index > last_mag_index)
|
||||
last_mag_index = latest_mag_index;
|
||||
mag_fuse_index = mag_fuse_index + 1;
|
||||
|
||||
|
||||
% output magnetic field length to help with diagnostics
|
||||
output.innovations.magLength(mag_fuse_index) = sqrt(dot(mag_data.field_ga(latest_mag_index,:),mag_data.field_ga(latest_mag_index,:)));
|
||||
|
||||
|
||||
% fuse magnetometer data
|
||||
if (param.fusion.magFuseMethod == 0 || param.fusion.magFuseMethod == 1)
|
||||
[states,covariance,magInnov,magInnovVar] = FuseMagnetometer(states,covariance,mag_data.field_ga(latest_mag_index,:),param.fusion.magFieldGate, param.fusion.magFieldError^2);
|
||||
|
||||
|
||||
% data logging
|
||||
output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
|
||||
output.innovations.magInnov(mag_fuse_index,:) = magInnov;
|
||||
output.innovations.magInnovVar(mag_fuse_index,:) = magInnovVar;
|
||||
|
||||
|
||||
if (param.fusion.magFuseMethod == 1)
|
||||
% fuse in the local declination value
|
||||
[states, covariance] = FuseMagDeclination(states, covariance, param.fusion.magDeclDeg*deg2rad);
|
||||
|
||||
|
||||
end
|
||||
|
||||
elseif (param.fusion.magFuseMethod == 2)
|
||||
% fuse magnetometer data as a single magnetic heading measurement
|
||||
[states, covariance, hdgInnov, hdgInnovVar] = FuseMagHeading(states, covariance, mag_data.field_ga(latest_mag_index,:), param.fusion.magDeclDeg*deg2rad, param.fusion.magHdgGate, param.fusion.magHdgError^2);
|
||||
|
||||
|
||||
% log data
|
||||
output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
|
||||
output.innovations.hdgInnov(mag_fuse_index) = hdgInnov;
|
||||
output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar;
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
% Check if optical flow use is being blocked by the user
|
||||
if ((local_time < param.control.flowOnTime) && (local_time > param.control.flowOffTime))
|
||||
flow_use_blocked = true;
|
||||
else
|
||||
flow_use_blocked = false;
|
||||
end
|
||||
|
||||
|
||||
% Attempt to use optical flow and range finder data if available and not blocked
|
||||
if (flowDataPresent && ~flow_use_blocked)
|
||||
|
||||
|
||||
% Get latest range finder data and gate to remove dropouts
|
||||
last_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (rng_data.dist(last_range_index) < param.fusion.rngValidMax)
|
||||
range = max(rng_data.dist(last_range_index) , param.fusion.rngValidMin);
|
||||
last_valid_range_time = local_time;
|
||||
end
|
||||
|
||||
|
||||
% Fuse optical flow data that has fallen behind the fusion time horizon if we have a valid range measurement
|
||||
latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
|
||||
|
||||
if (~isempty(latest_flow_index) && (latest_flow_index > last_flow_index) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout))
|
||||
last_flow_index = latest_flow_index;
|
||||
flow_fuse_index = flow_fuse_index + 1;
|
||||
last_drift_constrain_time = local_time;
|
||||
|
||||
|
||||
% fuse flow data
|
||||
flowRate = [flow_data.flowX(latest_flow_index);flow_data.flowY(latest_flow_index)];
|
||||
bodyRate = [flow_data.bodyX(latest_flow_index);flow_data.bodyY(latest_flow_index)];
|
||||
[states,covariance,flowInnov,flowInnovVar] = FuseOpticalFlow(states, covariance, flowRate, bodyRate, range, param.fusion.flowRateError^2, param.fusion.flowGate);
|
||||
|
||||
|
||||
% data logging
|
||||
output.innovations.flow_time_lapsed(flow_fuse_index) = local_time;
|
||||
output.innovations.flowInnov(flow_fuse_index,:) = flowInnov;
|
||||
output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar;
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
% Check if optical flow use is being blocked by the user
|
||||
if ((local_time < param.control.visoOnTime) && (local_time > param.control.visoOffTime))
|
||||
viso_use_blocked = true;
|
||||
else
|
||||
viso_use_blocked = false;
|
||||
end
|
||||
|
||||
|
||||
% attempt to use ZED camera visual odmetry data if available and not blocked
|
||||
if (visoDataPresent && ~viso_use_blocked)
|
||||
|
||||
|
||||
% Fuse ZED camera body frame odmometry data that has fallen behind the fusion time horizon
|
||||
latest_viso_index = find((viso_data.time_us - 1e6 * param.fusion.bodyVelTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_viso_index > last_viso_index)
|
||||
last_viso_index = latest_viso_index;
|
||||
viso_fuse_index = viso_fuse_index + 1;
|
||||
last_drift_constrain_time = local_time;
|
||||
|
||||
|
||||
% convert delta position measurements to velocity
|
||||
relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index);
|
||||
|
||||
|
||||
% convert quality metric to equivalent observation error
|
||||
% (this is a guess)
|
||||
quality = viso_data.qual(latest_viso_index);
|
||||
bodyVelError = param.fusion.bodyVelErrorMin * quality + param.fusion.bodyVelErrorMax * (1 - quality);
|
||||
|
||||
|
||||
% fuse measurements
|
||||
[states,covariance,bodyVelInnov,bodyVelInnovVar] = FuseBodyVel(states, covariance, relVelBodyMea, bodyVelError^2, param.fusion.bodyVelGate);
|
||||
|
||||
|
||||
% data logging
|
||||
output.innovations.bodyVel_time_lapsed(viso_fuse_index) = local_time;
|
||||
output.innovations.bodyVelInnov(viso_fuse_index,:) = bodyVelInnov;
|
||||
output.innovations.bodyVelInnovVar(viso_fuse_index,:) = bodyVelInnovVar;
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
% update average delta time estimate
|
||||
output.dt = dtCovInt / covIndex;
|
||||
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
|
|
@ -14,7 +14,7 @@ load '../TestData/APM/mag_data.mat';
|
|||
if exist('../TestData/APM/rng_data.mat','file') && exist('../TestData/APM/flow_data.mat','file')
|
||||
load '../TestData/APM/rng_data.mat';
|
||||
load '../TestData/APM/flow_data.mat';
|
||||
else
|
||||
else
|
||||
rng_data = [];
|
||||
flow_data = [];
|
||||
end
|
||||
|
@ -29,7 +29,7 @@ end
|
|||
% load default parameters
|
||||
run('SetParameters.m');
|
||||
|
||||
% run the filter replay
|
||||
% run the filter replay
|
||||
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data);
|
||||
|
||||
% generate and save output plots
|
||||
|
@ -43,4 +43,4 @@ fileName = '../OutputData/APM/ekf_replay_output.mat';
|
|||
if ~exist(folder,'dir')
|
||||
mkdir(folder);
|
||||
end
|
||||
save(fileName,'output');
|
||||
save(fileName,'output');
|
||||
|
|
|
@ -76,16 +76,16 @@ mag_strength = zeros(length(mag_meas),1);
|
|||
for i = 1:length(mag_meas)
|
||||
% subtract the offsets
|
||||
mag_corrected_5(:,i) = mag_meas(:,i) - offset;
|
||||
|
||||
|
||||
% correct the rotation
|
||||
mag_corrected_5(:,i) = rotation_correction * mag_corrected_5(:,i);
|
||||
|
||||
|
||||
% correct the scale factor
|
||||
mag_corrected_5(:,i) = mag_corrected_5(:,i) .* scale_correction;
|
||||
|
||||
|
||||
% calculate the residual
|
||||
mag_strength(i) = sqrt(dot(mag_corrected_5(:,i),mag_corrected_5(:,i)));
|
||||
|
||||
|
||||
end
|
||||
|
||||
% calculate the fit residual for fit option 5
|
||||
|
@ -104,19 +104,19 @@ angle_change_1 = zeros(length(mag_meas),1);
|
|||
for i = 1:length(mag_meas)
|
||||
% subtract the offsets
|
||||
mag_corrected_1(:,i) = mag_meas(:,i) - offset;
|
||||
|
||||
|
||||
% correct the rotation
|
||||
mag_corrected_1(:,i) = rotation_correction * mag_corrected_1(:,i);
|
||||
|
||||
|
||||
% correct the scale factor
|
||||
mag_corrected_1(:,i) = mag_corrected_1(:,i) .* scale_correction;
|
||||
|
||||
|
||||
% calculate the residual
|
||||
mag_strength(i) = sqrt(dot(mag_corrected_1(:,i),mag_corrected_1(:,i)));
|
||||
|
||||
|
||||
% calculate the angular change due to the fit
|
||||
angle_change_1(i) = atan2(norm(cross(mag_corrected_1(:,i),mag_meas(:,i))),dot(mag_corrected_1(:,i),mag_meas(:,i)));
|
||||
|
||||
|
||||
end
|
||||
|
||||
% calculate the fit residual for fit option 1
|
||||
|
@ -135,19 +135,19 @@ angle_change_0 = zeros(length(mag_meas),1);
|
|||
for i = 1:length(mag_meas)
|
||||
% subtract the offsets
|
||||
mag_corrected_0(:,i) = mag_meas(:,i) - offset;
|
||||
|
||||
|
||||
% correct the rotation
|
||||
mag_corrected_0(:,i) = rotation_correction * mag_corrected_0(:,i);
|
||||
|
||||
|
||||
% correct the scale factor
|
||||
mag_corrected_0(:,i) = mag_corrected_0(:,i) .* scale_correction;
|
||||
|
||||
|
||||
% calculate the residual
|
||||
mag_strength(i) = sqrt(dot(mag_corrected_0(:,i),mag_corrected_0(:,i)));
|
||||
|
||||
|
||||
% calculate the angular change due to the fit
|
||||
angle_change_0(i) = atan2(norm(cross(mag_corrected_0(:,i),mag_meas(:,i))),dot(mag_corrected_0(:,i),mag_meas(:,i)));
|
||||
|
||||
|
||||
end
|
||||
|
||||
% calculate the fit residual for fit option 0
|
||||
|
@ -155,7 +155,7 @@ fit_residual_0 = mag_strength - mean(mag_strength);
|
|||
|
||||
%% calculate the residual for uncorrected data
|
||||
for i = 1:length(mag_meas)
|
||||
mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i)));
|
||||
mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i)));
|
||||
end
|
||||
uncorrected_residual = mag_strength - mean(mag_strength);
|
||||
|
||||
|
|
|
@ -1,61 +1,61 @@
|
|||
function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin)
|
||||
function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin)
|
||||
|
||||
% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points
|
||||
% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points
|
||||
|
||||
% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z)
|
||||
% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z)
|
||||
|
||||
% optional flag f, default to 0 (fitting of rotated ellipsoid)
|
||||
% optional flag f, default to 0 (fitting of rotated ellipsoid)
|
||||
|
||||
x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end;
|
||||
x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end;
|
||||
|
||||
if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid)
|
||||
if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid)
|
||||
|
||||
elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid)
|
||||
elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid)
|
||||
|
||||
elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y
|
||||
elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y
|
||||
|
||||
elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z
|
||||
elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z
|
||||
|
||||
elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z
|
||||
elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z
|
||||
|
||||
elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere)
|
||||
elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere)
|
||||
|
||||
end;
|
||||
|
||||
v = (D'*D)\(D'*ones(length(x),1)); % least square fitting
|
||||
v = (D'*D)\(D'*ones(length(x),1)); % least square fitting
|
||||
|
||||
if f==0, % rotated ellipsoid
|
||||
if f==0, % rotated ellipsoid
|
||||
|
||||
A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ];
|
||||
A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ];
|
||||
|
||||
ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid
|
||||
ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid
|
||||
|
||||
Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0)
|
||||
Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0)
|
||||
|
||||
[rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain)
|
||||
[rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain)
|
||||
|
||||
gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid
|
||||
gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid
|
||||
|
||||
else % non-rotated ellipsoid
|
||||
else % non-rotated ellipsoid
|
||||
|
||||
if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
|
||||
if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
|
||||
|
||||
elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
|
||||
elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
|
||||
|
||||
elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||
elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||
|
||||
elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||
elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||
|
||||
elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere
|
||||
elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere
|
||||
|
||||
end;
|
||||
end;
|
||||
|
||||
ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid
|
||||
ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid
|
||||
|
||||
rotM=eye(3); % eigenvectors (rotation), identity = no rotation
|
||||
rotM=eye(3); % eigenvectors (rotation), identity = no rotation
|
||||
|
||||
g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3));
|
||||
g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3));
|
||||
|
||||
gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale)
|
||||
gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale)
|
||||
|
||||
end;
|
||||
|
|
|
@ -166,7 +166,7 @@ cd ../;
|
|||
%% ------ SECTION 3: Ground Truth Data (STIL only, optional) ------
|
||||
|
||||
if exist('attitude_file','var') && exist('localpos_file','var') && exist('globalpos_file','var')
|
||||
|
||||
|
||||
%- Import Attitude data from text file
|
||||
opts = delimitedTextImportOptions("NumVariables", 13);
|
||||
opts.DataLines = [2, Inf];
|
||||
|
|
|
@ -101,8 +101,8 @@ Finally copy the generated .mat files into the /EKF_replay/TestData/PX4 director
|
|||
5) Execute ‘SetParameterDefaults’ at the command prompt to load the default filter tuning parameter struct ‘param’ into the workspace. The defaults have been set to provide robust estimation across the entire data set, not optimised for accuracy.
|
||||
|
||||
|
||||
6) Replay the data by running either the replay_apm_data.m, replay_px4_data.m or if you have px4 optical flow data, the replay_px4_optflow_data.m script file.
|
||||
6) Replay the data by running either the replay_apm_data.m, replay_px4_data.m or if you have px4 optical flow data, the replay_px4_optflow_data.m script file.
|
||||
|
||||
Output plots are saved as .png files in the ‘…/EKF_replay/OutputPlots/‘ directory.
|
||||
|
||||
Output data is written to the Matlab workspace in the ‘output’ struct.
|
||||
Output data is written to the Matlab workspace in the ‘output’ struct.
|
||||
|
|
|
@ -96,20 +96,20 @@ else
|
|||
end
|
||||
centerLineTimeFull = get(lines(k),'XData');
|
||||
centerLineDataFull = get(lines(k),'YData');
|
||||
|
||||
|
||||
startTime = max(centerLineTimeFull(1),dataSrc.Tsec(1));
|
||||
endTime = min(centerLineTimeFull(end),dataSrc.Tsec(end));
|
||||
plotTimeFull = dataSrc.Tsec(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime);
|
||||
plotDataFull = dataSrc.(varNames{k})(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime);
|
||||
|
||||
|
||||
centerLineTime = centerLineTimeFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime);
|
||||
centerLineData = centerLineDataFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime);
|
||||
|
||||
|
||||
plotTime = linspace(startTime,endTime,250);
|
||||
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;
|
||||
|
|
|
@ -59,7 +59,7 @@ while 1
|
|||
fprintf('%.0f%%\n',percentDone);
|
||||
nextPrint = nextPrint + 5;
|
||||
end
|
||||
|
||||
|
||||
chunk = fread(fid,BLOCK_SIZE,'uint8');
|
||||
if numel(chunk) == 0;
|
||||
break
|
||||
|
@ -74,7 +74,7 @@ while 1
|
|||
continue;
|
||||
end
|
||||
msg_type = buffer(ptr+2);
|
||||
|
||||
|
||||
if msg_type == MSG_TYPE_FORMAT
|
||||
if numel(buffer) - ptr <= MSG_FORMAT_PACKET_LEN
|
||||
break;
|
||||
|
@ -144,7 +144,7 @@ msg_type = thisBlock(1);
|
|||
msg_labels = strsplit(LOCAL_parse_string(thisBlock(23:end)),',');
|
||||
msg_struct = cell(1,numel(msg_format));
|
||||
msg_mults = zeros(1,numel(msg_format));
|
||||
|
||||
|
||||
for k = 1:numel(msg_format)
|
||||
info = FORMAT_TO_STRUCT{strcmp(msg_format(k),FORMAT_TO_STRUCT(:,1)),2};
|
||||
msg_struct{k} = info{1};
|
||||
|
|
|
@ -14,11 +14,11 @@
|
|||
% the X and Y sensor axes. These rates are motion compensated.
|
||||
|
||||
% A positive LOS X rate is a RH rotation of the image about the X sensor
|
||||
% axis, and is produced by either a positive ground relative velocity in
|
||||
% axis, and is produced by either a positive ground relative velocity in
|
||||
% the direction of the Y axis.
|
||||
|
||||
% A positive LOS Y rate is a RH rotation of the image about the Y sensor
|
||||
% axis, and is produced by either a negative ground relative velocity in
|
||||
% axis, and is produced by either a negative ground relative velocity in
|
||||
% the direction of the X axis.
|
||||
|
||||
% Range measurement aligned with the Z body axis (flat earth model assumed)
|
||||
|
|
|
@ -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")
|
||||
|
||||
|
|
|
@ -29,7 +29,7 @@ R_tas_init = 1.4**2
|
|||
|
||||
#########################################################################################################################
|
||||
|
||||
# define symbols: true airspeed, sidslip angle,
|
||||
# define symbols: true airspeed, sidslip angle,
|
||||
V, beta, yaw, groundspeed_body_x, groundspeed_body_y = symbols('V beta yaw vx_body vy_body')
|
||||
R_tas, R_beta, R_yaw = symbols('R_tas R_beta R_yaw')
|
||||
|
||||
|
@ -64,4 +64,4 @@ P_wind_earth_numeric = P_wind_earth_numeric.subs([(groundspeed_body_x, groundspe
|
|||
print('P[22][22] = ' + str(P_wind_earth_numeric[0,0]))
|
||||
print('P[22][23] = ' + str(P_wind_earth_numeric[0,1]))
|
||||
print('P[23][22] = ' + str(P_wind_earth_numeric[1,0]))
|
||||
print('P[23][23] = ' + str(P_wind_earth_numeric[1,1]))
|
||||
print('P[23][23] = ' + str(P_wind_earth_numeric[1,1]))
|
||||
|
|
|
@ -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