├── .classpath
├── .gitignore
├── .project
├── .settings
    ├── org.eclipse.jdt.core.prefs
    └── org.eclipse.jdt.ui.prefs
├── FilterTrackResult.xlsx
├── README.md
└── src
    ├── coordinateconvert
        ├── CoordConversion.java
        ├── CoordinateConversion.java
        └── coordconvertTest.java
    ├── kalmanfilter
        ├── 1d.txt
        ├── KalmanBasic.java
        ├── KalmanFilter.java
        ├── LoadData.java
        ├── kfTest.java
        ├── test1.csv
        └── test2.csv
    ├── leastsquare
        ├── LeastSquare.java
        └── lsqTest.java
    ├── particlefilter
        ├── FileWrite.java
        ├── LoadData.java
        ├── Particle.java
        ├── ParticleFilter.java
        ├── Utils.java
        ├── output.txt
        ├── pfTest.java
        ├── show.xlsx
        ├── test1.csv
        └── test2.csv
    ├── pedometer
        ├── StepDetector.java
        ├── acc.txt
        ├── acc1.txt
        └── pedometerTest.java
    ├── simulator
        ├── FileWrite.java
        ├── Simulate.java
        └── Utils.java
    └── util
        ├── Matrix.java
        ├── Point2D.java
        └── Vector.java


/.classpath:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <classpath>
3 | 	<classpathentry kind="src" path="src"/>
4 | 	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.7"/>
5 | 	<classpathentry kind="con" path="org.eclipse.jdt.junit.JUNIT_CONTAINER/4"/>
6 | 	<classpathentry kind="output" path="bin"/>
7 | </classpath>
8 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | <<<<<<< HEAD
 2 | *.class
 3 | *.xslx
 4 | =======
 5 | # Compiled class file
 6 | *.class
 7 | 
 8 | # Log file
 9 | *.log
10 | 
11 | # BlueJ files
12 | *.ctxt
13 | 
14 | >>>>>>> ee01c1537805d31b4c4e815db68937765ea896f5
15 | # Mobile Tools for Java (J2ME)
16 | .mtj.tmp/
17 | 
18 | # Package Files #
19 | *.jar
20 | *.war
21 | *.ear
22 | <<<<<<< HEAD
23 | =======
24 | *.zip
25 | *.tar.gz
26 | *.rar
27 | >>>>>>> ee01c1537805d31b4c4e815db68937765ea896f5
28 | 
29 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
30 | hs_err_pid*
31 | /bin/
32 | 


--------------------------------------------------------------------------------
/.project:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <projectDescription>
 3 | 	<name>JFunctions</name>
 4 | 	<comment></comment>
 5 | 	<projects>
 6 | 	</projects>
 7 | 	<buildSpec>
 8 | 		<buildCommand>
 9 | 			<name>org.eclipse.jdt.core.javabuilder</name>
10 | 			<arguments>
11 | 			</arguments>
12 | 		</buildCommand>
13 | 	</buildSpec>
14 | 	<natures>
15 | 		<nature>org.eclipse.jdt.core.javanature</nature>
16 | 	</natures>
17 | </projectDescription>
18 | 


--------------------------------------------------------------------------------
/.settings/org.eclipse.jdt.core.prefs:
--------------------------------------------------------------------------------
  1 | eclipse.preferences.version=1
  2 | org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
  3 | org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.7
  4 | org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
  5 | org.eclipse.jdt.core.compiler.compliance=1.7
  6 | org.eclipse.jdt.core.compiler.debug.lineNumber=generate
  7 | org.eclipse.jdt.core.compiler.debug.localVariable=generate
  8 | org.eclipse.jdt.core.compiler.debug.sourceFile=generate
  9 | org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
 10 | org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
 11 | org.eclipse.jdt.core.compiler.source=1.7
 12 | org.eclipse.jdt.core.formatter.align_type_members_on_columns=false
 13 | org.eclipse.jdt.core.formatter.alignment_for_arguments_in_allocation_expression=16
 14 | org.eclipse.jdt.core.formatter.alignment_for_arguments_in_annotation=0
 15 | org.eclipse.jdt.core.formatter.alignment_for_arguments_in_enum_constant=16
 16 | org.eclipse.jdt.core.formatter.alignment_for_arguments_in_explicit_constructor_call=16
 17 | org.eclipse.jdt.core.formatter.alignment_for_arguments_in_method_invocation=16
 18 | org.eclipse.jdt.core.formatter.alignment_for_arguments_in_qualified_allocation_expression=16
 19 | org.eclipse.jdt.core.formatter.alignment_for_assignment=0
 20 | org.eclipse.jdt.core.formatter.alignment_for_binary_expression=16
 21 | org.eclipse.jdt.core.formatter.alignment_for_compact_if=16
 22 | org.eclipse.jdt.core.formatter.alignment_for_conditional_expression=80
 23 | org.eclipse.jdt.core.formatter.alignment_for_enum_constants=0
 24 | org.eclipse.jdt.core.formatter.alignment_for_expressions_in_array_initializer=16
 25 | org.eclipse.jdt.core.formatter.alignment_for_method_declaration=0
 26 | org.eclipse.jdt.core.formatter.alignment_for_multiple_fields=16
 27 | org.eclipse.jdt.core.formatter.alignment_for_parameters_in_constructor_declaration=16
 28 | org.eclipse.jdt.core.formatter.alignment_for_parameters_in_method_declaration=16
 29 | org.eclipse.jdt.core.formatter.alignment_for_resources_in_try=80
 30 | org.eclipse.jdt.core.formatter.alignment_for_selector_in_method_invocation=16
 31 | org.eclipse.jdt.core.formatter.alignment_for_superclass_in_type_declaration=16
 32 | org.eclipse.jdt.core.formatter.alignment_for_superinterfaces_in_enum_declaration=16
 33 | org.eclipse.jdt.core.formatter.alignment_for_superinterfaces_in_type_declaration=16
 34 | org.eclipse.jdt.core.formatter.alignment_for_throws_clause_in_constructor_declaration=16
 35 | org.eclipse.jdt.core.formatter.alignment_for_throws_clause_in_method_declaration=16
 36 | org.eclipse.jdt.core.formatter.alignment_for_union_type_in_multicatch=16
 37 | org.eclipse.jdt.core.formatter.blank_lines_after_imports=1
 38 | org.eclipse.jdt.core.formatter.blank_lines_after_package=1
 39 | org.eclipse.jdt.core.formatter.blank_lines_before_field=0
 40 | org.eclipse.jdt.core.formatter.blank_lines_before_first_class_body_declaration=0
 41 | org.eclipse.jdt.core.formatter.blank_lines_before_imports=1
 42 | org.eclipse.jdt.core.formatter.blank_lines_before_member_type=1
 43 | org.eclipse.jdt.core.formatter.blank_lines_before_method=1
 44 | org.eclipse.jdt.core.formatter.blank_lines_before_new_chunk=1
 45 | org.eclipse.jdt.core.formatter.blank_lines_before_package=0
 46 | org.eclipse.jdt.core.formatter.blank_lines_between_import_groups=1
 47 | org.eclipse.jdt.core.formatter.blank_lines_between_type_declarations=1
 48 | org.eclipse.jdt.core.formatter.brace_position_for_annotation_type_declaration=next_line
 49 | org.eclipse.jdt.core.formatter.brace_position_for_anonymous_type_declaration=next_line
 50 | org.eclipse.jdt.core.formatter.brace_position_for_array_initializer=end_of_line
 51 | org.eclipse.jdt.core.formatter.brace_position_for_block=next_line
 52 | org.eclipse.jdt.core.formatter.brace_position_for_block_in_case=next_line
 53 | org.eclipse.jdt.core.formatter.brace_position_for_constructor_declaration=next_line
 54 | org.eclipse.jdt.core.formatter.brace_position_for_enum_constant=next_line
 55 | org.eclipse.jdt.core.formatter.brace_position_for_enum_declaration=next_line
 56 | org.eclipse.jdt.core.formatter.brace_position_for_lambda_body=end_of_line
 57 | org.eclipse.jdt.core.formatter.brace_position_for_method_declaration=next_line
 58 | org.eclipse.jdt.core.formatter.brace_position_for_switch=next_line
 59 | org.eclipse.jdt.core.formatter.brace_position_for_type_declaration=next_line
 60 | org.eclipse.jdt.core.formatter.comment.clear_blank_lines_in_block_comment=false
 61 | org.eclipse.jdt.core.formatter.comment.clear_blank_lines_in_javadoc_comment=false
 62 | org.eclipse.jdt.core.formatter.comment.format_block_comments=true
 63 | org.eclipse.jdt.core.formatter.comment.format_header=false
 64 | org.eclipse.jdt.core.formatter.comment.format_html=true
 65 | org.eclipse.jdt.core.formatter.comment.format_javadoc_comments=true
 66 | org.eclipse.jdt.core.formatter.comment.format_line_comments=false
 67 | org.eclipse.jdt.core.formatter.comment.format_source_code=true
 68 | org.eclipse.jdt.core.formatter.comment.indent_parameter_description=true
 69 | org.eclipse.jdt.core.formatter.comment.indent_root_tags=true
 70 | org.eclipse.jdt.core.formatter.comment.insert_new_line_before_root_tags=insert
 71 | org.eclipse.jdt.core.formatter.comment.insert_new_line_for_parameter=insert
 72 | org.eclipse.jdt.core.formatter.comment.line_length=120
 73 | org.eclipse.jdt.core.formatter.comment.new_lines_at_block_boundaries=true
 74 | org.eclipse.jdt.core.formatter.comment.new_lines_at_javadoc_boundaries=true
 75 | org.eclipse.jdt.core.formatter.comment.preserve_white_space_between_code_and_line_comments=true
 76 | org.eclipse.jdt.core.formatter.compact_else_if=true
 77 | org.eclipse.jdt.core.formatter.continuation_indentation=2
 78 | org.eclipse.jdt.core.formatter.continuation_indentation_for_array_initializer=2
 79 | org.eclipse.jdt.core.formatter.disabling_tag=@formatter\:off
 80 | org.eclipse.jdt.core.formatter.enabling_tag=@formatter\:on
 81 | org.eclipse.jdt.core.formatter.format_guardian_clause_on_one_line=false
 82 | org.eclipse.jdt.core.formatter.format_line_comment_starting_on_first_column=true
 83 | org.eclipse.jdt.core.formatter.indent_body_declarations_compare_to_annotation_declaration_header=true
 84 | org.eclipse.jdt.core.formatter.indent_body_declarations_compare_to_enum_constant_header=true
 85 | org.eclipse.jdt.core.formatter.indent_body_declarations_compare_to_enum_declaration_header=true
 86 | org.eclipse.jdt.core.formatter.indent_body_declarations_compare_to_type_header=true
 87 | org.eclipse.jdt.core.formatter.indent_breaks_compare_to_cases=true
 88 | org.eclipse.jdt.core.formatter.indent_empty_lines=false
 89 | org.eclipse.jdt.core.formatter.indent_statements_compare_to_block=true
 90 | org.eclipse.jdt.core.formatter.indent_statements_compare_to_body=true
 91 | org.eclipse.jdt.core.formatter.indent_switchstatements_compare_to_cases=true
 92 | org.eclipse.jdt.core.formatter.indent_switchstatements_compare_to_switch=true
 93 | org.eclipse.jdt.core.formatter.indentation.size=4
 94 | org.eclipse.jdt.core.formatter.insert_new_line_after_annotation_on_field=insert
 95 | org.eclipse.jdt.core.formatter.insert_new_line_after_annotation_on_local_variable=insert
 96 | org.eclipse.jdt.core.formatter.insert_new_line_after_annotation_on_method=insert
 97 | org.eclipse.jdt.core.formatter.insert_new_line_after_annotation_on_package=insert
 98 | org.eclipse.jdt.core.formatter.insert_new_line_after_annotation_on_parameter=do not insert
 99 | org.eclipse.jdt.core.formatter.insert_new_line_after_annotation_on_type=insert
100 | org.eclipse.jdt.core.formatter.insert_new_line_after_label=do not insert
101 | org.eclipse.jdt.core.formatter.insert_new_line_after_opening_brace_in_array_initializer=do not insert
102 | org.eclipse.jdt.core.formatter.insert_new_line_after_type_annotation=do not insert
103 | org.eclipse.jdt.core.formatter.insert_new_line_at_end_of_file_if_missing=do not insert
104 | org.eclipse.jdt.core.formatter.insert_new_line_before_catch_in_try_statement=do not insert
105 | org.eclipse.jdt.core.formatter.insert_new_line_before_closing_brace_in_array_initializer=do not insert
106 | org.eclipse.jdt.core.formatter.insert_new_line_before_else_in_if_statement=do not insert
107 | org.eclipse.jdt.core.formatter.insert_new_line_before_finally_in_try_statement=do not insert
108 | org.eclipse.jdt.core.formatter.insert_new_line_before_while_in_do_statement=do not insert
109 | org.eclipse.jdt.core.formatter.insert_new_line_in_empty_annotation_declaration=insert
110 | org.eclipse.jdt.core.formatter.insert_new_line_in_empty_anonymous_type_declaration=insert
111 | org.eclipse.jdt.core.formatter.insert_new_line_in_empty_block=insert
112 | org.eclipse.jdt.core.formatter.insert_new_line_in_empty_enum_constant=insert
113 | org.eclipse.jdt.core.formatter.insert_new_line_in_empty_enum_declaration=insert
114 | org.eclipse.jdt.core.formatter.insert_new_line_in_empty_method_body=insert
115 | org.eclipse.jdt.core.formatter.insert_new_line_in_empty_type_declaration=insert
116 | org.eclipse.jdt.core.formatter.insert_space_after_and_in_type_parameter=insert
117 | org.eclipse.jdt.core.formatter.insert_space_after_assignment_operator=insert
118 | org.eclipse.jdt.core.formatter.insert_space_after_at_in_annotation=do not insert
119 | org.eclipse.jdt.core.formatter.insert_space_after_at_in_annotation_type_declaration=do not insert
120 | org.eclipse.jdt.core.formatter.insert_space_after_binary_operator=insert
121 | org.eclipse.jdt.core.formatter.insert_space_after_closing_angle_bracket_in_type_arguments=insert
122 | org.eclipse.jdt.core.formatter.insert_space_after_closing_angle_bracket_in_type_parameters=insert
123 | org.eclipse.jdt.core.formatter.insert_space_after_closing_brace_in_block=insert
124 | org.eclipse.jdt.core.formatter.insert_space_after_closing_paren_in_cast=insert
125 | org.eclipse.jdt.core.formatter.insert_space_after_colon_in_assert=insert
126 | org.eclipse.jdt.core.formatter.insert_space_after_colon_in_case=insert
127 | org.eclipse.jdt.core.formatter.insert_space_after_colon_in_conditional=insert
128 | org.eclipse.jdt.core.formatter.insert_space_after_colon_in_for=insert
129 | org.eclipse.jdt.core.formatter.insert_space_after_colon_in_labeled_statement=insert
130 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_allocation_expression=insert
131 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_annotation=insert
132 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_array_initializer=insert
133 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_constructor_declaration_parameters=insert
134 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_constructor_declaration_throws=insert
135 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_enum_constant_arguments=insert
136 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_enum_declarations=insert
137 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_explicitconstructorcall_arguments=insert
138 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_for_increments=insert
139 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_for_inits=insert
140 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_method_declaration_parameters=insert
141 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_method_declaration_throws=insert
142 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_method_invocation_arguments=insert
143 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_multiple_field_declarations=insert
144 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_multiple_local_declarations=insert
145 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_parameterized_type_reference=insert
146 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_superinterfaces=insert
147 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_type_arguments=insert
148 | org.eclipse.jdt.core.formatter.insert_space_after_comma_in_type_parameters=insert
149 | org.eclipse.jdt.core.formatter.insert_space_after_ellipsis=insert
150 | org.eclipse.jdt.core.formatter.insert_space_after_lambda_arrow=insert
151 | org.eclipse.jdt.core.formatter.insert_space_after_opening_angle_bracket_in_parameterized_type_reference=do not insert
152 | org.eclipse.jdt.core.formatter.insert_space_after_opening_angle_bracket_in_type_arguments=do not insert
153 | org.eclipse.jdt.core.formatter.insert_space_after_opening_angle_bracket_in_type_parameters=do not insert
154 | org.eclipse.jdt.core.formatter.insert_space_after_opening_brace_in_array_initializer=insert
155 | org.eclipse.jdt.core.formatter.insert_space_after_opening_bracket_in_array_allocation_expression=do not insert
156 | org.eclipse.jdt.core.formatter.insert_space_after_opening_bracket_in_array_reference=do not insert
157 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_annotation=do not insert
158 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_cast=do not insert
159 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_catch=do not insert
160 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_constructor_declaration=do not insert
161 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_enum_constant=do not insert
162 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_for=do not insert
163 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_if=do not insert
164 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_method_declaration=do not insert
165 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_method_invocation=do not insert
166 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_parenthesized_expression=do not insert
167 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_switch=do not insert
168 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_synchronized=do not insert
169 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_try=do not insert
170 | org.eclipse.jdt.core.formatter.insert_space_after_opening_paren_in_while=do not insert
171 | org.eclipse.jdt.core.formatter.insert_space_after_postfix_operator=do not insert
172 | org.eclipse.jdt.core.formatter.insert_space_after_prefix_operator=do not insert
173 | org.eclipse.jdt.core.formatter.insert_space_after_question_in_conditional=insert
174 | org.eclipse.jdt.core.formatter.insert_space_after_question_in_wildcard=do not insert
175 | org.eclipse.jdt.core.formatter.insert_space_after_semicolon_in_for=insert
176 | org.eclipse.jdt.core.formatter.insert_space_after_semicolon_in_try_resources=insert
177 | org.eclipse.jdt.core.formatter.insert_space_after_unary_operator=do not insert
178 | org.eclipse.jdt.core.formatter.insert_space_before_and_in_type_parameter=insert
179 | org.eclipse.jdt.core.formatter.insert_space_before_assignment_operator=insert
180 | org.eclipse.jdt.core.formatter.insert_space_before_at_in_annotation_type_declaration=insert
181 | org.eclipse.jdt.core.formatter.insert_space_before_binary_operator=insert
182 | org.eclipse.jdt.core.formatter.insert_space_before_closing_angle_bracket_in_parameterized_type_reference=do not insert
183 | org.eclipse.jdt.core.formatter.insert_space_before_closing_angle_bracket_in_type_arguments=do not insert
184 | org.eclipse.jdt.core.formatter.insert_space_before_closing_angle_bracket_in_type_parameters=do not insert
185 | org.eclipse.jdt.core.formatter.insert_space_before_closing_brace_in_array_initializer=insert
186 | org.eclipse.jdt.core.formatter.insert_space_before_closing_bracket_in_array_allocation_expression=do not insert
187 | org.eclipse.jdt.core.formatter.insert_space_before_closing_bracket_in_array_reference=do not insert
188 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_annotation=do not insert
189 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_cast=do not insert
190 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_catch=do not insert
191 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_constructor_declaration=do not insert
192 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_enum_constant=do not insert
193 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_for=do not insert
194 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_if=do not insert
195 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_method_declaration=do not insert
196 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_method_invocation=do not insert
197 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_parenthesized_expression=do not insert
198 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_switch=do not insert
199 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_synchronized=do not insert
200 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_try=do not insert
201 | org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_while=do not insert
202 | org.eclipse.jdt.core.formatter.insert_space_before_colon_in_assert=insert
203 | org.eclipse.jdt.core.formatter.insert_space_before_colon_in_case=do not insert
204 | org.eclipse.jdt.core.formatter.insert_space_before_colon_in_conditional=insert
205 | org.eclipse.jdt.core.formatter.insert_space_before_colon_in_default=do not insert
206 | org.eclipse.jdt.core.formatter.insert_space_before_colon_in_for=insert
207 | org.eclipse.jdt.core.formatter.insert_space_before_colon_in_labeled_statement=do not insert
208 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_allocation_expression=do not insert
209 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_annotation=do not insert
210 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_array_initializer=do not insert
211 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_constructor_declaration_parameters=do not insert
212 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_constructor_declaration_throws=do not insert
213 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_enum_constant_arguments=do not insert
214 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_enum_declarations=do not insert
215 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_explicitconstructorcall_arguments=do not insert
216 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_for_increments=do not insert
217 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_for_inits=do not insert
218 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_method_declaration_parameters=do not insert
219 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_method_declaration_throws=do not insert
220 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_method_invocation_arguments=do not insert
221 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_multiple_field_declarations=do not insert
222 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_multiple_local_declarations=do not insert
223 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_parameterized_type_reference=do not insert
224 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_superinterfaces=do not insert
225 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_type_arguments=do not insert
226 | org.eclipse.jdt.core.formatter.insert_space_before_comma_in_type_parameters=do not insert
227 | org.eclipse.jdt.core.formatter.insert_space_before_ellipsis=do not insert
228 | org.eclipse.jdt.core.formatter.insert_space_before_lambda_arrow=insert
229 | org.eclipse.jdt.core.formatter.insert_space_before_opening_angle_bracket_in_parameterized_type_reference=do not insert
230 | org.eclipse.jdt.core.formatter.insert_space_before_opening_angle_bracket_in_type_arguments=do not insert
231 | org.eclipse.jdt.core.formatter.insert_space_before_opening_angle_bracket_in_type_parameters=do not insert
232 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_annotation_type_declaration=insert
233 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_anonymous_type_declaration=insert
234 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_array_initializer=insert
235 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_block=insert
236 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_constructor_declaration=insert
237 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_enum_constant=insert
238 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_enum_declaration=insert
239 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_method_declaration=insert
240 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_switch=insert
241 | org.eclipse.jdt.core.formatter.insert_space_before_opening_brace_in_type_declaration=insert
242 | org.eclipse.jdt.core.formatter.insert_space_before_opening_bracket_in_array_allocation_expression=do not insert
243 | org.eclipse.jdt.core.formatter.insert_space_before_opening_bracket_in_array_reference=do not insert
244 | org.eclipse.jdt.core.formatter.insert_space_before_opening_bracket_in_array_type_reference=do not insert
245 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_annotation=do not insert
246 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_annotation_type_member_declaration=do not insert
247 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_catch=insert
248 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_constructor_declaration=do not insert
249 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_enum_constant=do not insert
250 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_for=insert
251 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_if=insert
252 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_method_declaration=do not insert
253 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_method_invocation=do not insert
254 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_parenthesized_expression=do not insert
255 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_switch=insert
256 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_synchronized=insert
257 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_try=insert
258 | org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_while=insert
259 | org.eclipse.jdt.core.formatter.insert_space_before_parenthesized_expression_in_return=insert
260 | org.eclipse.jdt.core.formatter.insert_space_before_parenthesized_expression_in_throw=insert
261 | org.eclipse.jdt.core.formatter.insert_space_before_postfix_operator=do not insert
262 | org.eclipse.jdt.core.formatter.insert_space_before_prefix_operator=do not insert
263 | org.eclipse.jdt.core.formatter.insert_space_before_question_in_conditional=insert
264 | org.eclipse.jdt.core.formatter.insert_space_before_question_in_wildcard=do not insert
265 | org.eclipse.jdt.core.formatter.insert_space_before_semicolon=do not insert
266 | org.eclipse.jdt.core.formatter.insert_space_before_semicolon_in_for=do not insert
267 | org.eclipse.jdt.core.formatter.insert_space_before_semicolon_in_try_resources=do not insert
268 | org.eclipse.jdt.core.formatter.insert_space_before_unary_operator=do not insert
269 | org.eclipse.jdt.core.formatter.insert_space_between_brackets_in_array_type_reference=do not insert
270 | org.eclipse.jdt.core.formatter.insert_space_between_empty_braces_in_array_initializer=do not insert
271 | org.eclipse.jdt.core.formatter.insert_space_between_empty_brackets_in_array_allocation_expression=do not insert
272 | org.eclipse.jdt.core.formatter.insert_space_between_empty_parens_in_annotation_type_member_declaration=do not insert
273 | org.eclipse.jdt.core.formatter.insert_space_between_empty_parens_in_constructor_declaration=do not insert
274 | org.eclipse.jdt.core.formatter.insert_space_between_empty_parens_in_enum_constant=do not insert
275 | org.eclipse.jdt.core.formatter.insert_space_between_empty_parens_in_method_declaration=do not insert
276 | org.eclipse.jdt.core.formatter.insert_space_between_empty_parens_in_method_invocation=do not insert
277 | org.eclipse.jdt.core.formatter.join_lines_in_comments=true
278 | org.eclipse.jdt.core.formatter.join_wrapped_lines=false
279 | org.eclipse.jdt.core.formatter.keep_else_statement_on_same_line=false
280 | org.eclipse.jdt.core.formatter.keep_empty_array_initializer_on_one_line=false
281 | org.eclipse.jdt.core.formatter.keep_imple_if_on_one_line=false
282 | org.eclipse.jdt.core.formatter.keep_then_statement_on_same_line=false
283 | org.eclipse.jdt.core.formatter.lineSplit=120
284 | org.eclipse.jdt.core.formatter.never_indent_block_comments_on_first_column=false
285 | org.eclipse.jdt.core.formatter.never_indent_line_comments_on_first_column=false
286 | org.eclipse.jdt.core.formatter.number_of_blank_lines_at_beginning_of_method_body=0
287 | org.eclipse.jdt.core.formatter.number_of_empty_lines_to_preserve=1
288 | org.eclipse.jdt.core.formatter.put_empty_statement_on_new_line=true
289 | org.eclipse.jdt.core.formatter.tabulation.char=tab
290 | org.eclipse.jdt.core.formatter.tabulation.size=4
291 | org.eclipse.jdt.core.formatter.use_on_off_tags=false
292 | org.eclipse.jdt.core.formatter.use_tabs_only_for_leading_indentations=false
293 | org.eclipse.jdt.core.formatter.wrap_before_binary_operator=true
294 | org.eclipse.jdt.core.formatter.wrap_before_or_operator_multicatch=true
295 | org.eclipse.jdt.core.formatter.wrap_outer_expressions_when_nested=true
296 | org.eclipse.jdt.core.javaFormatter=org.eclipse.jdt.core.defaultJavaFormatter
297 | 


--------------------------------------------------------------------------------
/.settings/org.eclipse.jdt.ui.prefs:
--------------------------------------------------------------------------------
1 | eclipse.preferences.version=1
2 | formatter_profile=_Eclipse_Leslie
3 | formatter_settings_version=12
4 | 


--------------------------------------------------------------------------------
/FilterTrackResult.xlsx:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/FilterTrackResult.xlsx


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # JAlgorithm
2 | Some algorthm used in indoor localization and filter.
3 | Include:
4 |  #coordinate covert
5 |  #kalman filter
6 |  #particle filter
7 |  #pedometer
8 | 


--------------------------------------------------------------------------------
/src/coordinateconvert/CoordConversion.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/coordinateconvert/CoordConversion.java


--------------------------------------------------------------------------------
/src/coordinateconvert/CoordinateConversion.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/coordinateconvert/CoordinateConversion.java


--------------------------------------------------------------------------------
/src/coordinateconvert/coordconvertTest.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/coordinateconvert/coordconvertTest.java


--------------------------------------------------------------------------------
/src/kalmanfilter/1d.txt:
--------------------------------------------------------------------------------
 1 | 0.21667525
 2 | 0.47524905
 3 | 0.676362
 4 | 0.22625206
 5 | 0.31244332
 6 | 0.2741361
 7 | 0.14963761
 8 | 0.130484
 9 | 0.33638534
10 | 0.39863458
11 | 0.2789245
12 | 0.30765492
13 | 0.37948096
14 | 0.29807812
15 | 0.16400282
16 | 0.073023155
17 | 0.25498247
18 | 0.2885013
19 | 0.3890578
20 | 0.36032736
21 | 0.15442601
22 | -0.051475335
23 | 0.130484
24 | 0.092176765
25 | -0.24779987
26 | -0.089782566
27 | -0.17118542
28 | -0.108936176
29 | 0.11133038
30 | -0.094570965
31 | -0.11372458
32 | -0.003591303
33 | -0.003591303
34 | 0.10175357
35 | 


--------------------------------------------------------------------------------
/src/kalmanfilter/KalmanBasic.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/kalmanfilter/KalmanBasic.java


--------------------------------------------------------------------------------
/src/kalmanfilter/KalmanFilter.java:
--------------------------------------------------------------------------------
 1 | package kalmanfilter;
 2 | 
 3 | import util.Matrix;
 4 | 
 5 | public class KalmanFilter extends KalmanBasic
 6 | {
 7 | 	
 8 | 	public KalmanFilter(int _obsvNum, int _stateNum)
 9 | 	{
10 | 		super(_obsvNum, _stateNum);
11 | 	}
12 | 
13 | 	@Override
14 | 	public void setStateTransitModelF(Matrix pdr)
15 | 	{
16 | 		Matrix stateTransitModelF= new Matrix(stateNum, 0);
17 | 		stateTransitModelF.setValue(0, 0, 1+(pdr.value(0, 0)/this.prioriState.value(0, 0)));
18 | 		stateTransitModelF.setValue(1, 1, 1+(pdr.value(1, 0)/this.prioriState.value(1, 0)));
19 | 	
20 | 		this.stateTransitModelF=stateTransitModelF;
21 | 	}
22 | 
23 | 	@Override
24 | 	void setDefaultModel()
25 | 	{
26 | 		double [][] observe={{1,0},{0,1}};
27 | 		setObsvModelH(new Matrix(observe));
28 | 		
29 | 		double [][] transit={{1,0},{0,1}};
30 | 		super.setStateTransitModelF(new Matrix(transit));
31 | 		
32 | 		setProcessNoiseCovQ(1);
33 | 		setObsvNoiseCovR(1);
34 | 	}
35 | 	
36 | 	@Override
37 | 	void setInitialState()
38 | 	{
39 | 		Matrix prioriState=new Matrix(stateNum,1,0);
40 | 		Matrix prioriErrorCovP=new Matrix(stateNum,1.0);//should set this big for converge.
41 | 	
42 | 		setCurrentState(prioriState, prioriErrorCovP);
43 | 	}
44 | 
45 | }
46 | 


--------------------------------------------------------------------------------
/src/kalmanfilter/LoadData.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/kalmanfilter/LoadData.java


--------------------------------------------------------------------------------
/src/kalmanfilter/kfTest.java:
--------------------------------------------------------------------------------
 1 | package kalmanfilter;
 2 | 
 3 | import java.util.List;
 4 | 
 5 | import particlefilter.LoadData;
 6 | import util.*;
 7 | 
 8 | public class kfTest
 9 | {
10 | 	public static void main(String[] args)
11 | 	{
12 | 		testKalman();
13 | 	}
14 | 
15 | 	public static void testKalman()
16 | 	{
17 | 		LoadData ld=new LoadData("src/kalmanfilter/test2.csv");
18 | 		List<Point2D> measurePDRList=ld.getMeasurePDRList();
19 | 		List<Point2D> measurePosList=ld.getMeasurePosList();
20 | 		List<Point2D> truePosList=ld.getTruePosList();
21 | 
22 | 		int num = measurePDRList.size();
23 | 		
24 | 		KalmanFilter kf = new KalmanFilter(2, 2);
25 | 		
26 | 		double[] xy={measurePosList.get(0).x,measurePosList.get(0).y};
27 | 		Matrix zz = new Matrix(xy).trans();
28 | 		kf.setCurrentState(zz,new Matrix(2,5));
29 | 
30 | 		System.out.printf("%8.3f %8.3f %8.3f %8.3f\n", xy[0],xy[1],0f,0f);
31 | 		// forward recursion
32 | 		for (int i = 1; i < num; i++)
33 | 		{
34 | 			Point2D trueP=truePosList.get(i);
35 | 			double[] xy1={measurePosList.get(i).x,measurePosList.get(i).y};
36 | 			Matrix z = new Matrix(xy1).trans();
37 | 			
38 | 			double [][] pdr={{ measurePDRList.get(i).x,0},{0, measurePDRList.get(i).y}};
39 | 			kf.setStateTransitModelF(new Matrix(pdr));
40 | 			Matrix state=kf.filter(z);
41 | 			System.out.printf("%8.3f %8.3f %8.3f %8.3f\n", state.value(0, 0),state.value(1, 0),trueP.x-state.value(0, 0),trueP.y-state.value(1, 0));
42 | 		}
43 | 	}
44 | 
45 | }
46 | 


--------------------------------------------------------------------------------
/src/kalmanfilter/test1.csv:
--------------------------------------------------------------------------------
 1 | pdr with error,,,absolute estimate,,,TRUE,
 2 | 0,0,,13210.01184,517747.1087,,13208.7,517748.91
 3 | -2.882237053,-0.107751963,,13204.94621,517750.8272,,13205.94,517748.74
 4 | -3.06216101,-0.074810557,,13205.1538,517751.1505,,13203.33,517748.7
 5 | -2.080765642,0.475554382,,13202.76187,517750.0813,,13201.29,517748.4
 6 | -0.447310579,3.225577775,,13201.73946,517749.7491,,13200.08,517751.43
 7 | -0.207037945,1.099670939,,13199.8012,517751.3594,,13199.52,517752.4
 8 | -0.489771136,0.959692441,,13198.49489,517752.7565,,13198.97,517753.36
 9 | 0.282197723,1.427882868,,13198.41333,517753.8295,,13199.01,517754.83
10 | 0.20367203,1.150041754,,13201.56739,517757.4529,,13199.19,517755.98
11 | 0.303038318,1.2186446,,13200.81106,517754.9492,,13198.94,517757.2
12 | 0.002993567,1.350588813,,13200.08571,517757.6734,,13198.54,517758.49
13 | 0.087051208,1.418604187,,13196.49224,517761.6534,,13198.29,517759.89
14 | 0.287302977,1.329457895,,13200.57267,517760.7466,,13198.33,517761.24
15 | 0.463040624,1.138393612,,13196.60887,517760.939,,13198.37,517762.52
16 | 0.307720904,1.41986669,,13199.40509,517765.7244,,13198.41,517763.99
17 | 0.500020809,1.255454901,,13196.63212,517763.9259,,13198.6,517765.39
18 | 0.081315705,1.429694738,,13200.42517,517765.7204,,13198.35,517766.8
19 | -0.110195592,1.344038858,,13195.99734,517766.4693,,13197.8,517768.03
20 | -0.380107449,1.706179669,,13199.28708,517771.1932,,13197.13,517769.63
21 | -3.041671315,0.942160198,,13195.42317,517769.6855,,13194.26,517770.69
22 | -2.313694441,2.096462154,,13193.92327,517769.9149,,13191.84,517772.32
23 | -2.445570082,0.280922506,,13188.99272,517770.235,,13189.09,517772.53
24 | -2.18820077,1.511473596,,13186.84336,517771.6682,,13186.65,517773.13
25 | -1.670585801,1.207795114,,13184.16498,517775.4645,,13184.79,517773.86
26 | -1.346962366,0.739544394,,13184.00931,517772.9916,,13183.07,517774.65
27 | -0.871990945,1.26533171,,13180.6505,517773.8957,,13181.94,517775.69
28 | -0.554885537,0.84231037,,13183.34785,517777.8212,,13181.24,517776.52
29 | -0.317779521,1.016938074,,13179.75717,517777.3872,,13180.98,517777.55
30 | 0.691059342,1.146278735,,13183.64684,517779.0771,,13181.16,517778.82
31 | 2.008524405,0.527229064,,13181.16652,517780.0134,,13182.5,517779.91
32 | 2.506278185,1.542810203,,13184.58733,517780.5599,,13185.15,517781.48
33 | 1.73233612,0.574587356,,13187.38633,517784.7035,,13186.2,517782.62
34 | 1.145225359,0.989280962,,13188.58543,517785.1316,,13187.25,517783.57
35 | 1.585298999,0.802694471,,13190.82738,517786.8997,,13188.58,517784.51
36 | 1.854568327,0.103825182,,13191.95676,517787.0367,,13190.34,517785.26
37 | 1.507901898,0.71857473,,13191.6213,517786.0196,,13192.1,517785.95
38 | 2.91383482,-0.400100341,,13194.1487,517784.9372,,13194.43,517786.57
39 | 2.461352953,0.487836662,,13194.20159,517789.098,,13196.63,517787.31
40 | 2.292480193,0.44906072,,13200.50343,517788.4357,,13198.97,517788.12
41 | 1.721152914,0.225694292,,13199.5511,517788.1756,,13201.01,517788.56
42 | 2.430063263,-0.210575686,,13204.60954,517787.2261,,13203.05,517788.73
43 | 2.178333617,0.181224609,,13206.79243,517787.0981,,13205.08,517788.96
44 | 2.062321277,-0.015028717,,13206.47526,517788.9386,,13207.26,517789.06
45 | 1.994784013,-0.362050867,,13210.80359,517787.9084,,13209.29,517789.04
46 | 1.577737739,-0.636040415,,13210.55057,517790.9046,,13211.17,517788.96
47 | 1.555374417,-0.270415735,,13214.53883,517787.0523,,13212.76,517788.95
48 | 1.412734827,-0.208455414,,13214.60969,517791.1926,,13213.92,517788.88
49 | 0.356454587,-0.017946969,,13216.28235,517789.6955,,13214.36,517788.87
50 | 0.280151346,0.170127322,,13214.6179,517789.4177,,13214.51,517789.12
51 | -0.012250119,0.526208936,,13214.78546,517788.1185,,13214.38,517789.63
52 | -1.540647005,0.894917039,,13211.04518,517787.8441,,13212.79,517789.9
53 | -3.819673477,0.47576835,,13209.87539,517787.8555,,13209.47,517790.13
54 | -2.088783281,2.530731772,,13206.69924,517793.4151,,13207.06,517792.07
55 | -2.198604566,1.61847168,,13205.59703,517794.8087,,13205.22,517793.62
56 | -1.695641004,0.638837461,,13205.54286,517792.3996,,13203.64,517794.09
57 | -1.661979875,4.560674984,,13203.32052,517795.8844,,13200.85,517797.97
58 | -1.43769479,2.561183232,,13196.15087,517801.9986,,13198.44,517799.9
59 | -1.32795207,2.05013819,,13198.52905,517802.2655,,13196.46,517801.52
60 | -0.803800567,2.443791101,,13195.28421,517802.7377,,13194.63,517803.4
61 | -1.121617283,1.79865308,,13195.97933,517803.797,,13193.53,517805.14
62 | -1.042916372,1.790592124,,13194.58064,517808.3911,,13192.56,517806.87
63 | 0.042755687,1.874978392,,13194.39847,517808.8612,,13192.32,517808.73
64 | 0.276199219,1.933785782,,13193.32248,517812.977,,13192.67,517810.65
65 | 1.917535177,1.456999623,,13195.69015,517812.7393,,13193.74,517812.49
66 | 2.562949332,1.450823088,,13195.86991,517813.8519,,13196.4,517814.13
67 | 1.385808777,0.660799046,,13198.12784,517812.8707,,13197.87,517815.07
68 | 1.81443682,-0.365648727,,13200.69932,517813.7154,,13199.62,517815.33
69 | 1.04104898,-0.854227672,,13203.48595,517816.4715,,13201.07,517814.9
70 | 1.239795657,-0.636754832,,13203.42086,517817.1235,,13202.37,517814.81
71 | 0.931906959,-0.626316667,,13204.36854,517813.21,,13203.52,517814.55
72 | 0.668824242,-1.5257656,,13204.17876,517813.1979,,13204.2,517813.01
73 | 0.862193417,-0.110824227,,13206.01111,517813.7477,,13205.06,517813.09
74 | 0.06610085,0.826721187,,13207.13692,517815.4814,,13205.09,517813.92
75 | -2.255859495,-2.622971232,,13205.35396,517812.0035,,13203.83,517810.48
76 | -2.176554462,1.738852812,,13200.75835,517811.9754,,13200.66,517811.28
77 | -1.725643407,-1.179725915,,13201.06605,517812.1106,,13198.6,517810.02
78 | -1.421297057,1.037627097,,13195.0172,517811.9749,,13196.73,517810.67
79 | -1.892865807,0.079365079,,13193.10734,517811.2001,,13194.83,517810.05
80 | -2.102840453,-0.025414512,,13191.60522,517808.4514,,13193.08,517809.62
81 | -1.70232498,-0.365564085,,13189.27556,517808.105,,13191.62,517809.31
82 | -1.464439133,0.225164348,,13192.19757,517806.9739,,13189.88,517809.26
83 | -1.574496935,1.177489784,,13186.52159,517811.5645,,13188.02,517810.05
84 | -1.403743138,1.66705943,,13184.09617,517812.693,,13186.16,517811.16
85 | -1.352843829,1.444872878,,13186.7698,517810.751,,13184.45,517812.13
86 | -1.091307957,1.079545974,,13182.27989,517811.7325,,13183.02,517812.85
87 | -0.870796477,3.731409415,,13181.00064,517817.9021,,13180.79,517815.95
88 | -1.684912389,1.400345903,,13179.70985,517815.9754,,13178.65,517816.87
89 | -1.254554441,2.596673808,,13177.43759,517821.4126,,13176.97,517819.12
90 | -1.756721278,1.915035926,,13176.4752,517821.3957,,13175.43,517820.93
91 | -0.517595923,2.002550656,,13171.72975,517822.3641,,13174.03,517822.48
92 | -0.4096,1.881858349,,13175.12806,517821.9698,,13172.77,517823.97
93 | -0.266771872,1.339752263,,13171.12018,517825.0972,,13172.52,517825.32
94 | -0.187108843,1.502317919,,13173.83141,517828.1105,,13171.98,517826.73
95 | -0.09219082,1.215956815,,13172.27417,517828.821,,13171.43,517827.82
96 | 0.371099841,0.250358378,,13171.38395,517826.783,,13171.87,517828.13
97 | -0.10581856,0.847116329,,13171.6368,517830.5803,,13171.75,517828.98
98 | 


--------------------------------------------------------------------------------
/src/kalmanfilter/test2.csv:
--------------------------------------------------------------------------------
 1 | pdr with error,,,absolute estimate,,,TRUE,
 2 | 0,0,,13210.01184,517747.1087,,13208.7,517748.91
 3 | -2.882237053,-0.107751963,,13204.94621,517750.8272,,13205.94,517748.74
 4 | -3.06216101,-0.074810557,,13205.1538,517751.1505,,13203.33,517748.7
 5 | -2.080765642,0.475554382,,13202.76187,517750.0813,,13201.29,517748.4
 6 | -0.447310579,3.225577775,,13201.73946,517749.7491,,13200.08,517751.43
 7 | -0.207037945,1.099670939,,13199.8012,517751.3594,,13199.52,517752.4
 8 | -0.489771136,0.959692441,,13198.49489,517752.7565,,13198.97,517753.36
 9 | 0.282197723,1.427882868,,13198.41333,517753.8295,,13199.01,517754.83
10 | 0.20367203,1.150041754,,13201.56739,517757.4529,,13199.19,517755.98
11 | 0.303038318,1.2186446,,13200.81106,517754.9492,,13198.94,517757.2
12 | 0.002993567,1.350588813,,13200.08571,517757.6734,,13198.54,517758.49
13 | 0.087051208,1.418604187,,13196.49224,517761.6534,,13198.29,517759.89
14 | 0.287302977,1.329457895,,13200.57267,517760.7466,,13198.33,517761.24
15 | 0.463040624,1.138393612,,13196.60887,517760.939,,13198.37,517762.52
16 | 0.307720904,1.41986669,,13199.40509,517765.7244,,13198.41,517763.99
17 | 0.500020809,1.255454901,,13196.63212,517763.9259,,13198.6,517765.39
18 | 0.081315705,1.429694738,,13200.42517,517765.7204,,13198.35,517766.8
19 | -0.110195592,1.344038858,,13195.99734,517766.4693,,13197.8,517768.03
20 | -0.380107449,1.706179669,,13199.28708,517771.1932,,13197.13,517769.63
21 | -3.041671315,0.942160198,,13195.42317,517769.6855,,13194.26,517770.69
22 | -2.313694441,2.096462154,,13193.92327,517769.9149,,13191.84,517772.32
23 | -2.445570082,0.280922506,,13188.99272,517770.235,,13189.09,517772.53
24 | -2.18820077,1.511473596,,13186.84336,517771.6682,,13186.65,517773.13
25 | -1.670585801,1.207795114,,13184.16498,517775.4645,,13184.79,517773.86
26 | -1.346962366,0.739544394,,13184.00931,517772.9916,,13183.07,517774.65
27 | -0.871990945,1.26533171,,13180.6505,517773.8957,,13181.94,517775.69
28 | -0.554885537,0.84231037,,13183.34785,517777.8212,,13181.24,517776.52
29 | -0.317779521,1.016938074,,13179.75717,517777.3872,,13180.98,517777.55
30 | 0.691059342,1.146278735,,13183.64684,517779.0771,,13181.16,517778.82
31 | 2.008524405,0.527229064,,13181.16652,517780.0134,,13182.5,517779.91
32 | 2.506278185,1.542810203,,13184.58733,517780.5599,,13185.15,517781.48
33 | 1.73233612,0.574587356,,13187.38633,517784.7035,,13186.2,517782.62
34 | 1.145225359,0.989280962,,13188.58543,517785.1316,,13187.25,517783.57
35 | 1.585298999,0.802694471,,13190.82738,517786.8997,,13188.58,517784.51
36 | 1.854568327,0.103825182,,13191.95676,517787.0367,,13190.34,517785.26
37 | 1.507901898,0.71857473,,13191.6213,517786.0196,,13192.1,517785.95
38 | 2.91383482,-0.400100341,,13194.1487,517784.9372,,13194.43,517786.57
39 | 2.461352953,0.487836662,,13194.20159,517789.098,,13196.63,517787.31
40 | 2.292480193,0.44906072,,13200.50343,517788.4357,,13198.97,517788.12
41 | 1.721152914,0.225694292,,13199.5511,517788.1756,,13201.01,517788.56
42 | 2.430063263,-0.210575686,,13204.60954,517787.2261,,13203.05,517788.73
43 | 2.178333617,0.181224609,,13206.79243,517787.0981,,13205.08,517788.96
44 | 2.062321277,-0.015028717,,13206.47526,517788.9386,,13207.26,517789.06
45 | 1.994784013,-0.362050867,,13210.80359,517787.9084,,13209.29,517789.04
46 | 1.577737739,-0.636040415,,13210.55057,517790.9046,,13211.17,517788.96
47 | 1.555374417,-0.270415735,,13214.53883,517787.0523,,13212.76,517788.95
48 | 1.412734827,-0.208455414,,13214.60969,517791.1926,,13213.92,517788.88
49 | 0.356454587,-0.017946969,,13216.28235,517789.6955,,13214.36,517788.87
50 | 0.280151346,0.170127322,,13214.6179,517789.4177,,13214.51,517789.12
51 | -0.012250119,0.526208936,,13214.78546,517788.1185,,13214.38,517789.63
52 | -1.540647005,0.894917039,,13211.04518,517787.8441,,13212.79,517789.9
53 | -3.819673477,0.47576835,,13209.87539,517787.8555,,13209.47,517790.13
54 | -2.088783281,2.530731772,,13206.69924,517793.4151,,13207.06,517792.07
55 | -2.198604566,1.61847168,,13205.59703,517794.8087,,13205.22,517793.62
56 | -1.695641004,0.638837461,,13205.54286,517792.3996,,13203.64,517794.09
57 | -1.661979875,4.560674984,,13203.32052,517795.8844,,13200.85,517797.97
58 | -1.43769479,2.561183232,,13196.15087,517801.9986,,13198.44,517799.9
59 | -1.32795207,2.05013819,,13198.52905,517802.2655,,13196.46,517801.52
60 | -0.803800567,2.443791101,,13195.28421,517802.7377,,13194.63,517803.4
61 | -1.121617283,1.79865308,,13195.97933,517803.797,,13193.53,517805.14
62 | -1.042916372,1.790592124,,13194.58064,517808.3911,,13192.56,517806.87
63 | 0.042755687,1.874978392,,13194.39847,517808.8612,,13192.32,517808.73
64 | 0.276199219,1.933785782,,13193.32248,517812.977,,13192.67,517810.65
65 | 1.917535177,1.456999623,,13195.69015,517812.7393,,13193.74,517812.49
66 | 2.562949332,1.450823088,,13195.86991,517813.8519,,13196.4,517814.13
67 | 1.385808777,0.660799046,,13198.12784,517812.8707,,13197.87,517815.07
68 | 1.81443682,-0.365648727,,13200.69932,517813.7154,,13199.62,517815.33
69 | 1.04104898,-0.854227672,,13203.48595,517816.4715,,13201.07,517814.9
70 | 1.239795657,-0.636754832,,13203.42086,517817.1235,,13202.37,517814.81
71 | 0.931906959,-0.626316667,,13204.36854,517813.21,,13203.52,517814.55
72 | 0.668824242,-1.5257656,,13204.17876,517813.1979,,13204.2,517813.01
73 | 0.862193417,-0.110824227,,13206.01111,517813.7477,,13205.06,517813.09
74 | 0.06610085,0.826721187,,13207.13692,517815.4814,,13205.09,517813.92
75 | -2.255859495,-2.622971232,,13205.35396,517812.0035,,13203.83,517810.48
76 | -2.176554462,1.738852812,,13200.75835,517811.9754,,13200.66,517811.28
77 | -1.725643407,-1.179725915,,13201.06605,517812.1106,,13198.6,517810.02
78 | -1.421297057,1.037627097,,13195.0172,517811.9749,,13196.73,517810.67
79 | -1.892865807,0.079365079,,13193.10734,517811.2001,,13194.83,517810.05
80 | -2.102840453,-0.025414512,,13191.60522,517808.4514,,13193.08,517809.62
81 | -1.70232498,-0.365564085,,13189.27556,517808.105,,13191.62,517809.31
82 | -1.464439133,0.225164348,,13192.19757,517806.9739,,13189.88,517809.26
83 | -1.574496935,1.177489784,,13186.52159,517811.5645,,13188.02,517810.05
84 | -1.403743138,1.66705943,,13184.09617,517812.693,,13186.16,517811.16
85 | -1.352843829,1.444872878,,13186.7698,517810.751,,13184.45,517812.13
86 | -1.091307957,1.079545974,,13182.27989,517811.7325,,13183.02,517812.85
87 | -0.870796477,3.731409415,,13181.00064,517817.9021,,13180.79,517815.95
88 | -1.684912389,1.400345903,,13179.70985,517815.9754,,13178.65,517816.87
89 | -1.254554441,2.596673808,,13177.43759,517821.4126,,13176.97,517819.12
90 | -1.756721278,1.915035926,,13176.4752,517821.3957,,13175.43,517820.93
91 | -0.517595923,2.002550656,,13171.72975,517822.3641,,13174.03,517822.48
92 | -0.4096,1.881858349,,13175.12806,517821.9698,,13172.77,517823.97
93 | -0.266771872,1.339752263,,13171.12018,517825.0972,,13172.52,517825.32
94 | -0.187108843,1.502317919,,13173.83141,517828.1105,,13171.98,517826.73
95 | -0.09219082,1.215956815,,13172.27417,517828.821,,13171.43,517827.82
96 | 0.371099841,0.250358378,,13171.38395,517826.783,,13171.87,517828.13
97 | -0.10581856,0.847116329,,13171.6368,517830.5803,,13171.75,517828.98
98 | 


--------------------------------------------------------------------------------
/src/leastsquare/LeastSquare.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/leastsquare/LeastSquare.java


--------------------------------------------------------------------------------
/src/leastsquare/lsqTest.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/leastsquare/lsqTest.java


--------------------------------------------------------------------------------
/src/particlefilter/FileWrite.java:
--------------------------------------------------------------------------------
 1 | package particlefilter;
 2 | 
 3 | import java.io.BufferedWriter;
 4 | import java.io.File;
 5 | import java.io.FileWriter;
 6 | import java.io.IOException;
 7 | 
 8 | public class FileWrite {
 9 | 	private String filePath;
10 | 	private BufferedWriter bw;
11 | 	
12 | 	public FileWrite(String filePath)
13 | 	{
14 | 		this.filePath=filePath;
15 | 		initial();
16 | 	}
17 | 	
18 | 	private void initial()
19 | 	{
20 | 		File file = new File(filePath);
21 | 		if (!file.exists()) {
22 | 			try {
23 | 				file.createNewFile();
24 | 			} catch (IOException e) {
25 | 				// TODO Auto-generated catch block
26 | 				e.printStackTrace();
27 | 			}
28 | 		}
29 | 		FileWriter fw = null;
30 | 		try {
31 | 			fw = new FileWriter(file.getAbsoluteFile());
32 | 		} catch (IOException e) {
33 | 			// TODO Auto-generated catch block
34 | 			e.printStackTrace();
35 | 		}
36 | 		bw = new BufferedWriter(fw);
37 | 	}
38 | 	
39 | 	public void write(String content)
40 | 	{
41 | 		try {
42 | 			bw.write(content);
43 | 		} catch (IOException e) {
44 | 			// TODO Auto-generated catch block
45 | 			e.printStackTrace();
46 | 		}
47 | 	}
48 | 	
49 | 	public void closeStream()
50 | 	{
51 | 		try {
52 | 			bw.close();
53 | 			//System.out.print("file closed");
54 | 		} catch (IOException e) {
55 | 			// TODO Auto-generated catch block
56 | 			e.printStackTrace();
57 | 		}
58 | 	}
59 | }
60 | 


--------------------------------------------------------------------------------
/src/particlefilter/LoadData.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/particlefilter/LoadData.java


--------------------------------------------------------------------------------
/src/particlefilter/Particle.java:
--------------------------------------------------------------------------------
  1 | package particlefilter;
  2 | 
  3 | import java.util.Random;
  4 | 
  5 | import simulator.Utils;
  6 | import util.Point2D;
  7 | 
  8 | /**
  9 |  *
 10 |  * @author  LeslieXong
 11 |  */
 12 | public class Particle extends Point2D{
 13 |     public float senseStd; //stand deviation
 14 |     public float direction;
 15 |     public float worldWidth;
 16 |     public float worldHeight;
 17 |     public double weight = 0;
 18 |     public Point2D[] landmarks;
 19 |     private Random random;
 20 |     
 21 |     /**
 22 |      * Default constructor for a particle
 23 |      * 
 24 |      * @param landmarks Point array of landmark points for the particle
 25 |      * @param width width of the particle's world in pixels
 26 |      * @param height height of the particle's world in pixels
 27 |      */
 28 |     public Particle(Point2D[] landmarks, float width, float height) {
 29 |         this.landmarks = landmarks;
 30 |         this.worldWidth = width;
 31 |         this.worldHeight = height;
 32 |         random = new Random();
 33 |         x = random.nextFloat() * width;
 34 |         y = random.nextFloat() * height;
 35 |         direction = random.nextFloat() * 2f * ((float)Math.PI);
 36 |         senseStd = 0f;        
 37 |     }
 38 | 
 39 | 	/**
 40 |      * Sets the position of the particle and its relative probability
 41 |      * 
 42 |      * @param x new x position of the particle
 43 |      * @param y new y position of the particle
 44 |      * @param direction new orientation of the particle, in radians
 45 |      * @param prob new probability of the particle between 0 and 1
 46 |      * @throws Exception 
 47 |      */
 48 |     public void set(float x, float y, float direction, double prob) throws Exception {
 49 |         if(x < 0 || x >= worldWidth) {
 50 |             throw new Exception("X coordinate out of bounds");
 51 |         }
 52 |         if(y < 0 || y >= worldHeight) {
 53 |             throw new Exception("Y coordinate out of bounds");
 54 |         }
 55 |         if(direction < 0 || direction >= 2 * Math.PI) {
 56 |             throw new Exception("X coordinate out of bounds");
 57 |         }
 58 |         this.x = x;
 59 |         this.y = y;
 60 |         this.direction = direction;
 61 |         this.weight = prob;
 62 |     }
 63 |     
 64 |     /**
 65 |      * Sets the noise of the particles measurements and movements
 66 |      * 
 67 |      * @param Fnoise noise of particle in forward movement
 68 |      * @param Onoise noise of particle in direction measurement
 69 |      * @param Snoise noise of particle in sensing position
 70 |      */
 71 |     public void setSenseNoise(float Snoise) {
 72 |         this.senseStd = Snoise;
 73 |     }
 74 |     
 75 |  
 76 |     /**
 77 |      * Moves the particle's position(propagate)
 78 |      * TODO INS error should consider system error other than random error
 79 |      * @param direction value, in radians
 80 |      * @param forward move value, must be >= 0
 81 |      */
 82 |     public void move(float direction, float forward) throws Exception {
 83 |         if(forward < 0) {
 84 |             throw new Exception("target cannot move backwards");
 85 |         }
 86 |         
 87 |         this.direction = circle(direction, 2f * (float)Math.PI);;
 88 |        
 89 |         x += Math.cos(direction) * forward;
 90 |         y += Math.sin(direction) * forward;
 91 |         x = circle(x, worldWidth);   //TODO in real application this should be removed
 92 |         y = circle(y, worldHeight);
 93 |     }
 94 |     
 95 |     /**
 96 |      * Calculates the probability of particle based on all measurement
 97 |      * 
 98 |      * @param measurement distance measurements 
 99 |      */
100 |     public void likelihood(float[] measurement) {
101 |         double likeli = 1.0;
102 |         for(int i=0;i<landmarks.length;i++) {
103 |             float dist = (float) Utils.distance(x, y, landmarks[i].x, landmarks[i].y);            
104 |             likeli *= Utils.gaussianPdf(dist, senseStd, measurement[i]);            
105 |         }      
106 |         weight = likeli;
107 |     }
108 |     
109 |     /**
110 |      * just use the nearest on beacon as measurement
111 |      * @param measurement
112 |      */
113 |     public void likelihood2(float[] measurement) {
114 |     	float min=measurement[0];
115 |     	int minIndex=0;
116 |     	for (int i=0;i<measurement.length;i++)
117 | 		{
118 |     		if (measurement[i]<min)
119 |     		{
120 |     			min=measurement[i];
121 |     			minIndex=i;
122 |     		}
123 | 		}
124 |     	
125 |     	float dist = (float) Utils.distance(x, y, landmarks[minIndex].x, landmarks[minIndex].y);   
126 |         weight =  Utils.gaussianPdf(dist, senseStd, measurement[minIndex]);  ;
127 |     }
128 |     
129 |     private float circle(float num, float length) {         
130 |         while(num > length - 1) num -= length;
131 |         while(num < 0) num += length;
132 |         return num;       
133 |     }
134 |     
135 |     @Override
136 |     public String toString() {
137 |         return "[x=" + x + " y=" + y + " orient=" + Math.toDegrees(direction) + " prob=" +weight +  "]";
138 |     }
139 |     
140 | }


--------------------------------------------------------------------------------
/src/particlefilter/ParticleFilter.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/particlefilter/ParticleFilter.java


--------------------------------------------------------------------------------
/src/particlefilter/Utils.java:
--------------------------------------------------------------------------------
  1 | package particlefilter;
  2 | 
  3 | //some of the code is borrowed from here
  4 | //http://www.movable-type.co.uk/scripts/latlong.html
  5 | 
  6 | public final class Utils {
  7 | 	//helper classes
  8 |     //static double R = 6371; //Mean radius of earth in km
  9 | 	//semi-minor axis (Earth's center to a pole distance)
 10 | 	static double earth_b =  1000*6356.7523142; 
 11 | 	//semi-major axis (Earth's center to equator distance)
 12 | 	static double earth_a =  1000*6378.1370; 
 13 | 	//HARDCODED for Chicago
 14 | 	static double R = Utils.geocentric_radius(41.88194); 
 15 | 	
 16 | 	//helper class for storing lat and lon
 17 | 	public static final class GeoPoint {
 18 | 		public double lat;
 19 | 		public double lon;
 20 | 		public GeoPoint(double _lat, double _lon)
 21 | 		{
 22 | 			this.lat = _lat;
 23 | 			this.lon = _lon;
 24 | 		}
 25 | 	}
 26 | 	
 27 | 	public static GeoPoint proj_to_world_mercator(double lat, double lon) {
 28 | 		double x,y;
 29 | 		lat = Utils.to_rad(lat);
 30 | 		lon = Utils.to_rad(lon);
 31 | 		double lon0 = Utils.to_rad(-87.627778);
 32 | 		
 33 | 		x = Utils.earth_a*(lon - lon0);
 34 | 		y = Utils.earth_a*(Math.log(Math.tan(lat/2 + Math.PI/4)));
 35 | 		return new GeoPoint(x,y);
 36 | 		
 37 | 	}
 38 | 	
 39 | 	//Calculates geocentric radius of the earth at a given latitude
 40 | 	//http://en.wikipedia.org/wiki/Earth_radius
 41 | 	public static double geocentric_radius(double lat){
 42 | 		lat = Utils.to_rad(lat);
 43 | 		
 44 | 		
 45 | 		return Math.sqrt((Math.pow(earth_a*earth_a*Math.cos(lat),2) +Math.pow(earth_b*earth_b*Math.sin(lat),2))/
 46 | 				(Math.pow(earth_a*Math.cos(lat),2) +Math.pow(earth_b*Math.sin(lat),2)) );
 47 | 	}
 48 | 	//Converts degrees to radian
 49 | 	public static double to_rad(double angle)
 50 | 	{
 51 | 		return ((angle%360)*Math.PI / 180.0);
 52 | 	}
 53 | //	converts radian to degree
 54 | 	public static double to_deg(double rad)
 55 | 	{
 56 | 		return rad*180.0 / Math.PI;
 57 | 	}
 58 | 	//returns the value of Gaussian pdf with mu=mean and std^2=varience ec=valuated at x
 59 | 	public static double dnorm(double x, double mean, double varience)
 60 | 	{
 61 | 		return Math.exp(-Math.pow(x-mean,2)/(2*varience))/(Math.sqrt(varience)*Math.sqrt(2*Math.PI));
 62 | 	}
 63 | 	
 64 | 	//Updates the lat and lon by moving the point by d meters in the direction given by bearing
 65 | 	public static void move_lat_lon(GeoPoint pt, double d, double bearing)
 66 | 	{
 67 | 		double lat1, lat2, lon1,lon2, theta, delta;
 68 |  		lat1 = Utils.to_rad(pt.lat);
 69 | 		lon1 = Utils.to_rad(pt.lon);
 70 | 		theta = Utils.to_rad(bearing);
 71 | 		delta = d/(R);
 72 | 		
 73 | 		lat2 = Math.asin(Math.sin(lat1)*Math.cos(delta) + 
 74 | 				Math.cos(lat1)*Math.sin(delta)*Math.cos(theta));
 75 | 		lon2 = lon1 + Math.atan2(Math.sin(theta)*Math.sin(delta)*Math.cos(lat1), Math.cos(delta) - Math.sin(lat1)*Math.sin(lat2));
 76 | 		lon2 = (lon2 + 3*Math.PI) % (2*Math.PI) - Math.PI; // normalize to -180..180
 77 | 		pt.lat = Utils.to_deg(lat2);
 78 | 		pt.lon = Utils.to_deg(lon2);
 79 | 	}
 80 | 	
 81 | 	//Converts degrees, minutes and seconds to decomal form
 82 | 	public static double to_decimal_degrees(int degree, int minute, int second)
 83 | 	{
 84 | 		return degree + minute/60.0 + second/3600.0;
 85 | 	}
 86 | 	//returns distance between 2 points
 87 | 	public static double distance(double lat1, double lon1, double lat2, double lon2)
 88 | 	{
 89 | 		lat1 = Utils.to_rad(lat1);
 90 | 		lat2 = Utils.to_rad(lat2);
 91 | 		lon1 = Utils.to_rad(lon1);
 92 | 		lon2 = Utils.to_rad(lon2);
 93 | 		double dlat = lat2-lat1;
 94 | 		double dlon = lon2-lon1;
 95 | 		double a = Math.sin(dlat/2) * Math.sin(dlat/2) + 
 96 | 				Math.cos(lat1)*Math.cos(lat2)*Math.sin(dlon/2)*Math.sin(dlon/2);
 97 | 		double c = 2 * Math.atan2(Math.sqrt(a),Math.sqrt(1-a));
 98 | 		return R*c;		
 99 | 	}
100 | 	
101 | 	//calculates the resulting bearing when going from pt1 to pt2
102 | 	public static double bearing(double lat1, double lon1, double lat2, double lon2)
103 | 	{
104 | 		double theta2 = Utils.to_rad(lat1);
105 | 		double theta1 = Utils.to_rad(lat2);
106 | 		double dlon = Utils.to_rad(lon1-lon2);
107 | 		double y = Math.sin(dlon)*Math.cos(theta2);
108 | 		double x = Math.cos(theta1)*Math.sin(theta2) - 
109 | 				Math.sin(theta1)*Math.cos(theta2)*Math.cos(dlon);
110 | 		double theta = Math.atan2(y, x);
111 | 		return (Utils.to_deg(theta)+180) % 360;
112 | 	}
113 | }
114 | 


--------------------------------------------------------------------------------
/src/particlefilter/output.txt:
--------------------------------------------------------------------------------
 1 | 0,13209.895205745848,517747.1043027146,-1.1952057458474883,1.8056972853955813
 2 | 1,13205.481694721899,517748.2290515212,0.45830527810176136,0.510948478768114
 3 | 2,13203.42808478821,517748.8409506309,-0.09808478820923483,-0.14095063088461757
 4 | 3,13200.822556778638,517749.5036964441,0.46744322136328265,-1.1036964440718293
 5 | 4,13200.78872423042,517752.14521546673,-0.7087242304205574,-0.7152154667419381
 6 | 5,13200.163550853382,517753.70146010566,-0.6435508533813845,-1.3014601056347601
 7 | 6,13199.298159898915,517754.7522676965,-0.3281598989160557,-1.3922676965012215
 8 | 7,13199.473993319603,517756.2355395021,-0.4639933196031052,-1.4055395020986907
 9 | 8,13199.989957392945,517757.8467666116,-0.7999573929446342,-1.8667666116380133
10 | 9,13200.431004367281,517758.53015880793,-1.491004367280766,-1.3301588079193607
11 | 10,13200.444678760377,517760.1933491859,-1.9046787603765551,-1.7033491859328933
12 | 11,13200.039564766617,517762.10986186616,-1.7495647666164587,-2.2198618661495857
13 | 12,13200.568194577778,517763.32919374254,-2.2381945777779038,-2.08919374254765
14 | 13,13200.658823700396,517764.1975892879,-2.288823700395369,-1.6775892878649756
15 | 14,13201.056440900142,517766.465791977,-2.646440900141897,-2.4757919770199805
16 | 15,13201.215511946803,517767.308996284,-2.615511946802144,-1.9189962839591317
17 | 16,13201.437935522634,517768.78588565986,-3.0879355226334155,-1.98588565987302
18 | 17,13200.961514242037,517770.14592445287,-3.1615142420378106,-2.1159244528389536
19 | 18,13200.450179801232,517772.4588009969,-3.3201798012323707,-2.8288009968819097
20 | 19,13196.436791172784,517773.0062129134,-2.176791172783851,-2.3162129133706912
21 | 20,13194.533042080866,517774.6113640729,-2.69304208086578,-2.291364072880242
22 | 21,13189.763142386655,517774.6876133905,-0.6731423866549449,-2.1576133904745802
23 | 22,13187.282615013179,517776.2581720745,-0.63261501317902,-3.1281720745027997
24 | 23,13184.574849309165,517777.9954255107,0.21515069083579874,-4.135425510699861
25 | 24,13183.632362144619,517778.2893230375,-0.5623621446193283,-3.6393230374669656
26 | 25,13182.037497468214,517779.68087085744,-0.0974974682139873,-3.9908708574366756
27 | 26,13182.088201799224,517780.57245778485,-0.8482017992246256,-4.052457784826402
28 | 27,13181.34206502314,517781.73175747885,-0.3620650231405307,-4.181757478858344
29 | 28,13182.755565109255,517782.81212214584,-1.5955651092554035,-3.99212214583531
30 | 29,13184.317991763806,517783.27963477874,-1.8179917638062761,-3.369634778762702
31 | 30,13186.913558082764,517784.7354278499,-1.7635580827645754,-3.255427849944681
32 | 31,13189.178733513078,517785.5854574842,-2.9787335130768042,-2.9654574841842987
33 | 32,13190.281413431638,517786.6479506768,-3.031413431637702,-3.0779506767867133
34 | 33,13192.091094698124,517787.6992316327,-3.5110946981239977,-3.189231632684823
35 | 34,13194.001650843962,517787.64796506625,-3.6616508439619793,-2.3879650662420318
36 | 35,13195.051011401105,517788.24474898673,-2.9510114011045516,-2.29474898672197
37 | 36,13197.844949276498,517787.6349809436,-3.414949276497282,-1.0649809435708448
38 | 37,13199.792164568895,517788.35338208335,-3.162164568895605,-1.0433820833568461
39 | 38,13202.826831157094,517788.85586401407,-3.856831157094348,-0.7358640140737407
40 | 39,13204.1314450727,517789.0138535253,-3.121445072700226,-0.45385352527955547
41 | 40,13207.021965092694,517788.65078132047,-3.971965092694518,0.07921867951517925
42 | 41,13209.339233613613,517788.7899907957,-4.259233613613105,0.17000920430291444
43 | 42,13211.006402333776,517788.83974337304,-3.746402333776132,0.22025662695523351
44 | 43,13213.562202782696,517788.32603883167,-4.272202782694876,0.7139611683087423
45 | 44,13214.592127855145,517787.9206728115,-3.422127855144936,1.039327188511379
46 | 45,13216.689914369928,517787.4834470274,-3.929914369928156,1.4665529726189561
47 | 46,13217.910218479317,517787.45786675997,-3.9902184793172637,1.4221332400338724
48 | 47,13218.22810096277,517787.54094853194,-3.868100962768949,1.3290514680556953
49 | 48,13218.106987949503,517787.82236639434,-3.5969879495023633,1.2976336056599393
50 | 49,13217.675807899948,517788.61827638303,-3.295807899949068,1.0117236169753596
51 | 50,13214.479154481234,517790.06557596725,-1.689154481233345,-0.16557596722850576
52 | 51,13210.248940350455,517790.25690250675,-0.7789403504557413,-0.12690250674495474
53 | 52,13207.199344719667,517793.91692368296,-0.13934471966786077,-1.8469236829550937
54 | 53,13205.078488389103,517795.66705817526,0.14151161089648667,-2.0470581752597354
55 | 54,13204.27119471775,517795.45355783874,-0.6311947177509865,-1.3635578387184069
56 | 55,13202.788390817248,517799.7282476843,-1.9383908172476367,-1.75824768433813
57 | 56,13199.38403007595,517803.97455463,-0.9440300759488309,-4.074554629973136
58 | 57,13198.668178017144,517805.31472803134,-2.2081780171447463,-3.794728031323757
59 | 58,13197.523251753924,517807.60301942856,-2.893251753925142,-4.203019428532571
60 | 59,13196.689286897134,517809.190132348,-3.1592868971329153,-4.050132347969338
61 | 60,13195.286431154422,517811.5772336584,-2.7264311544222437,-4.7072336584096774
62 | 61,13195.542070070487,517813.184441485,-3.2220700704874616,-4.454441484995186
63 | 62,13195.50568315238,517815.7229237751,-2.8356831523797155,-5.072923775063828
64 | 63,13197.540839771751,517816.80559170595,-3.8008397717512707,-4.315591705963016
65 | 64,13199.664690877149,517818.062596231,-3.2646908771494054,-3.9325962309958413
66 | 65,13201.356500006186,517818.48969037755,-3.4865000061854516,-3.4196903775446117
67 | 66,13203.878343871738,517817.90883157856,-4.258343871737452,-2.5788315785466693
68 | 67,13205.099604373378,517816.66036375164,-4.029604373377879,-1.760363751614932
69 | 68,13206.006718509205,517816.3324485716,-3.6367185092040017,-1.5224485715734772
70 | 69,13207.084553783092,517815.254994546,-3.564553783091469,-0.7049945460166782
71 | 70,13207.534703069643,517813.49819442275,-3.3347030696422735,-0.48819442273816094
72 | 71,13208.495918192577,517813.5878162854,-3.4359181925774465,-0.4978162853512913
73 | 72,13208.306508853655,517815.0395607773,-3.2165088536548865,-1.1195607773261145
74 | 73,13205.512236686187,517811.9239736789,-1.682236686187025,-1.4439736789208837
75 | 74,13202.007077597542,517813.55975375243,-1.347077597542011,-2.27975375240203
76 | 75,13200.414841230186,517812.1216324479,-1.8148412301852659,-2.1016324478550814
77 | 76,13197.264166061786,517812.99041733285,-0.5341660617868911,-2.32041733287042
78 | 77,13193.765577523787,517812.47723415925,1.0644224762127124,-2.42723415925866
79 | 78,13190.979658073227,517811.6518395682,2.1003419267726713,-2.0318395682261325
80 | 79,13188.411437142346,517810.72554487386,3.2085628576551244,-1.415544873860199
81 | 80,13188.248421554617,517810.8292103409,1.6315784453818196,-1.5692103409091942
82 | 81,13185.696947793504,517812.58931483637,2.323052206496868,-2.539314836380072
83 | 82,13183.84184355415,517814.3696071042,2.318156445850036,-3.2096071042469703
84 | 83,13183.306193833165,517815.09567345376,1.1438061668359296,-2.965673453756608
85 | 84,13181.579168377177,517816.15715436,1.44083162282368,-3.307154359994456
86 | 85,13180.602486685995,517820.1796767866,0.18751331400562776,-4.229676786577329
87 | 86,13178.789144953786,517821.1002806306,-0.13914495378594438,-4.230280630581547
88 | 87,13177.319413813939,517824.238311196,-0.3494138139394636,-5.118311195983551
89 | 88,13175.632240024857,517825.70145763,-0.2022400248570193,-4.771457630035002
90 | 89,13174.469910992151,517827.7126909236,-0.4399109921505442,-5.2326909236144274
91 | 90,13174.64314693627,517829.0432507329,-1.8731469362701318,-5.073250732908491
92 | 91,13173.771724292694,517830.7625827975,-1.251724292693325,-5.442582797491923
93 | 92,13173.868771363255,517832.5780759186,-1.888771363255728,-5.848075918620452
94 | 93,13173.656936589567,517833.8069464016,-2.2269365895663213,-5.986946401593741
95 | 94,13174.179018433226,517833.41432384786,-2.309018433224992,-5.284323847852647
96 | 95,13173.864883599148,517834.58181938267,-2.1148835991480155,-5.601819382689428
97 | 


--------------------------------------------------------------------------------
/src/particlefilter/pfTest.java:
--------------------------------------------------------------------------------
 1 | //package particlefilter;
 2 | //import java.util.ArrayList;
 3 | //import java.util.List;
 4 | //
 5 | //import util.*;
 6 | //
 7 | //public class test {
 8 | //	public static void TrackBoat() {
 9 | //		LoadData ld=new LoadData("src/particlefilter/test2.csv");
10 | //		List<Point2D> measurePDRList=ld.getMeasurePDRList();
11 | //		List<Point2D> measurePosList=ld.getMeasurePosList();
12 | //		List<Point2D> truePosList=ld.getTruePosList();
13 | //			
14 | //		int N = 500;  //particle number
15 | 
16 | //	}
17 | //
18 | //	public static void main(String[] args) {
19 | //		pfTest.test();
20 | //	}
21 | //}
22 | 


--------------------------------------------------------------------------------
/src/particlefilter/show.xlsx:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/particlefilter/show.xlsx


--------------------------------------------------------------------------------
/src/particlefilter/test1.csv:
--------------------------------------------------------------------------------
 1 | pdr with error,,,absolute estimate,,,TRUE,
 2 | 0,0,,13210.01184,517747.1087,,13208.7,517748.91
 3 | -2.882237053,-0.107751963,,13204.94621,517750.8272,,13205.94,517748.74
 4 | -3.06216101,-0.074810557,,13205.1538,517751.1505,,13203.33,517748.7
 5 | -2.080765642,0.475554382,,13202.76187,517750.0813,,13201.29,517748.4
 6 | -0.447310579,3.225577775,,13201.73946,517749.7491,,13200.08,517751.43
 7 | -0.207037945,1.099670939,,13199.8012,517751.3594,,13199.52,517752.4
 8 | -0.489771136,0.959692441,,13198.49489,517752.7565,,13198.97,517753.36
 9 | 0.282197723,1.427882868,,13198.41333,517753.8295,,13199.01,517754.83
10 | 0.20367203,1.150041754,,13201.56739,517757.4529,,13199.19,517755.98
11 | 0.303038318,1.2186446,,13200.81106,517754.9492,,13198.94,517757.2
12 | 0.002993567,1.350588813,,13200.08571,517757.6734,,13198.54,517758.49
13 | 0.087051208,1.418604187,,13196.49224,517761.6534,,13198.29,517759.89
14 | 0.287302977,1.329457895,,13200.57267,517760.7466,,13198.33,517761.24
15 | 0.463040624,1.138393612,,13196.60887,517760.939,,13198.37,517762.52
16 | 0.307720904,1.41986669,,13199.40509,517765.7244,,13198.41,517763.99
17 | 0.500020809,1.255454901,,13196.63212,517763.9259,,13198.6,517765.39
18 | 0.081315705,1.429694738,,13200.42517,517765.7204,,13198.35,517766.8
19 | -0.110195592,1.344038858,,13195.99734,517766.4693,,13197.8,517768.03
20 | -0.380107449,1.706179669,,13199.28708,517771.1932,,13197.13,517769.63
21 | -3.041671315,0.942160198,,13195.42317,517769.6855,,13194.26,517770.69
22 | -2.313694441,2.096462154,,13193.92327,517769.9149,,13191.84,517772.32
23 | -2.445570082,0.280922506,,13188.99272,517770.235,,13189.09,517772.53
24 | -2.18820077,1.511473596,,13186.84336,517771.6682,,13186.65,517773.13
25 | -1.670585801,1.207795114,,13184.16498,517775.4645,,13184.79,517773.86
26 | -1.346962366,0.739544394,,13184.00931,517772.9916,,13183.07,517774.65
27 | -0.871990945,1.26533171,,13180.6505,517773.8957,,13181.94,517775.69
28 | -0.554885537,0.84231037,,13183.34785,517777.8212,,13181.24,517776.52
29 | -0.317779521,1.016938074,,13179.75717,517777.3872,,13180.98,517777.55
30 | 0.691059342,1.146278735,,13183.64684,517779.0771,,13181.16,517778.82
31 | 2.008524405,0.527229064,,13181.16652,517780.0134,,13182.5,517779.91
32 | 2.506278185,1.542810203,,13184.58733,517780.5599,,13185.15,517781.48
33 | 1.73233612,0.574587356,,13187.38633,517784.7035,,13186.2,517782.62
34 | 1.145225359,0.989280962,,13188.58543,517785.1316,,13187.25,517783.57
35 | 1.585298999,0.802694471,,13190.82738,517786.8997,,13188.58,517784.51
36 | 1.854568327,0.103825182,,13191.95676,517787.0367,,13190.34,517785.26
37 | 1.507901898,0.71857473,,13191.6213,517786.0196,,13192.1,517785.95
38 | 2.91383482,-0.400100341,,13194.1487,517784.9372,,13194.43,517786.57
39 | 2.461352953,0.487836662,,13194.20159,517789.098,,13196.63,517787.31
40 | 2.292480193,0.44906072,,13200.50343,517788.4357,,13198.97,517788.12
41 | 1.721152914,0.225694292,,13199.5511,517788.1756,,13201.01,517788.56
42 | 2.430063263,-0.210575686,,13204.60954,517787.2261,,13203.05,517788.73
43 | 2.178333617,0.181224609,,13206.79243,517787.0981,,13205.08,517788.96
44 | 2.062321277,-0.015028717,,13206.47526,517788.9386,,13207.26,517789.06
45 | 1.994784013,-0.362050867,,13210.80359,517787.9084,,13209.29,517789.04
46 | 1.577737739,-0.636040415,,13210.55057,517790.9046,,13211.17,517788.96
47 | 1.555374417,-0.270415735,,13214.53883,517787.0523,,13212.76,517788.95
48 | 1.412734827,-0.208455414,,13214.60969,517791.1926,,13213.92,517788.88
49 | 0.356454587,-0.017946969,,13216.28235,517789.6955,,13214.36,517788.87
50 | 0.280151346,0.170127322,,13214.6179,517789.4177,,13214.51,517789.12
51 | -0.012250119,0.526208936,,13214.78546,517788.1185,,13214.38,517789.63
52 | -1.540647005,0.894917039,,13211.04518,517787.8441,,13212.79,517789.9
53 | -3.819673477,0.47576835,,13209.87539,517787.8555,,13209.47,517790.13
54 | -2.088783281,2.530731772,,13206.69924,517793.4151,,13207.06,517792.07
55 | -2.198604566,1.61847168,,13205.59703,517794.8087,,13205.22,517793.62
56 | -1.695641004,0.638837461,,13205.54286,517792.3996,,13203.64,517794.09
57 | -1.661979875,4.560674984,,13203.32052,517795.8844,,13200.85,517797.97
58 | -1.43769479,2.561183232,,13196.15087,517801.9986,,13198.44,517799.9
59 | -1.32795207,2.05013819,,13198.52905,517802.2655,,13196.46,517801.52
60 | -0.803800567,2.443791101,,13195.28421,517802.7377,,13194.63,517803.4
61 | -1.121617283,1.79865308,,13195.97933,517803.797,,13193.53,517805.14
62 | -1.042916372,1.790592124,,13194.58064,517808.3911,,13192.56,517806.87
63 | 0.042755687,1.874978392,,13194.39847,517808.8612,,13192.32,517808.73
64 | 0.276199219,1.933785782,,13193.32248,517812.977,,13192.67,517810.65
65 | 1.917535177,1.456999623,,13195.69015,517812.7393,,13193.74,517812.49
66 | 2.562949332,1.450823088,,13195.86991,517813.8519,,13196.4,517814.13
67 | 1.385808777,0.660799046,,13198.12784,517812.8707,,13197.87,517815.07
68 | 1.81443682,-0.365648727,,13200.69932,517813.7154,,13199.62,517815.33
69 | 1.04104898,-0.854227672,,13203.48595,517816.4715,,13201.07,517814.9
70 | 1.239795657,-0.636754832,,13203.42086,517817.1235,,13202.37,517814.81
71 | 0.931906959,-0.626316667,,13204.36854,517813.21,,13203.52,517814.55
72 | 0.668824242,-1.5257656,,13204.17876,517813.1979,,13204.2,517813.01
73 | 0.862193417,-0.110824227,,13206.01111,517813.7477,,13205.06,517813.09
74 | 0.06610085,0.826721187,,13207.13692,517815.4814,,13205.09,517813.92
75 | -2.255859495,-2.622971232,,13205.35396,517812.0035,,13203.83,517810.48
76 | -2.176554462,1.738852812,,13200.75835,517811.9754,,13200.66,517811.28
77 | -1.725643407,-1.179725915,,13201.06605,517812.1106,,13198.6,517810.02
78 | -1.421297057,1.037627097,,13195.0172,517811.9749,,13196.73,517810.67
79 | -1.892865807,0.079365079,,13193.10734,517811.2001,,13194.83,517810.05
80 | -2.102840453,-0.025414512,,13191.60522,517808.4514,,13193.08,517809.62
81 | -1.70232498,-0.365564085,,13189.27556,517808.105,,13191.62,517809.31
82 | -1.464439133,0.225164348,,13192.19757,517806.9739,,13189.88,517809.26
83 | -1.574496935,1.177489784,,13186.52159,517811.5645,,13188.02,517810.05
84 | -1.403743138,1.66705943,,13184.09617,517812.693,,13186.16,517811.16
85 | -1.352843829,1.444872878,,13186.7698,517810.751,,13184.45,517812.13
86 | -1.091307957,1.079545974,,13182.27989,517811.7325,,13183.02,517812.85
87 | -0.870796477,3.731409415,,13181.00064,517817.9021,,13180.79,517815.95
88 | -1.684912389,1.400345903,,13179.70985,517815.9754,,13178.65,517816.87
89 | -1.254554441,2.596673808,,13177.43759,517821.4126,,13176.97,517819.12
90 | -1.756721278,1.915035926,,13176.4752,517821.3957,,13175.43,517820.93
91 | -0.517595923,2.002550656,,13171.72975,517822.3641,,13174.03,517822.48
92 | -0.4096,1.881858349,,13175.12806,517821.9698,,13172.77,517823.97
93 | -0.266771872,1.339752263,,13171.12018,517825.0972,,13172.52,517825.32
94 | -0.187108843,1.502317919,,13173.83141,517828.1105,,13171.98,517826.73
95 | -0.09219082,1.215956815,,13172.27417,517828.821,,13171.43,517827.82
96 | 0.371099841,0.250358378,,13171.38395,517826.783,,13171.87,517828.13
97 | -0.10581856,0.847116329,,13171.6368,517830.5803,,13171.75,517828.98
98 | 


--------------------------------------------------------------------------------
/src/particlefilter/test2.csv:
--------------------------------------------------------------------------------
 1 | pdr with error,,,absolute estimate,,,TRUE,
 2 | 0,0,,13210.01184,517747.1087,,13208.7,517748.91
 3 | -2.882237053,-0.107751963,,13204.94621,517750.8272,,13205.94,517748.74
 4 | -3.06216101,-0.074810557,,13205.1538,517751.1505,,13203.33,517748.7
 5 | -2.080765642,0.475554382,,13202.76187,517750.0813,,13201.29,517748.4
 6 | -0.447310579,3.225577775,,13201.73946,517749.7491,,13200.08,517751.43
 7 | -0.207037945,1.099670939,,13199.8012,517751.3594,,13199.52,517752.4
 8 | -0.489771136,0.959692441,,13198.49489,517752.7565,,13198.97,517753.36
 9 | 0.282197723,1.427882868,,13198.41333,517753.8295,,13199.01,517754.83
10 | 0.20367203,1.150041754,,13201.56739,517757.4529,,13199.19,517755.98
11 | 0.303038318,1.2186446,,13200.81106,517754.9492,,13198.94,517757.2
12 | 0.002993567,1.350588813,,13200.08571,517757.6734,,13198.54,517758.49
13 | 0.087051208,1.418604187,,13196.49224,517761.6534,,13198.29,517759.89
14 | 0.287302977,1.329457895,,13200.57267,517760.7466,,13198.33,517761.24
15 | 0.463040624,1.138393612,,13196.60887,517760.939,,13198.37,517762.52
16 | 0.307720904,1.41986669,,13199.40509,517765.7244,,13198.41,517763.99
17 | 0.500020809,1.255454901,,13196.63212,517763.9259,,13198.6,517765.39
18 | 0.081315705,1.429694738,,13200.42517,517765.7204,,13198.35,517766.8
19 | -0.110195592,1.344038858,,13195.99734,517766.4693,,13197.8,517768.03
20 | -0.380107449,1.706179669,,13199.28708,517771.1932,,13197.13,517769.63
21 | -3.041671315,0.942160198,,13195.42317,517769.6855,,13194.26,517770.69
22 | -2.313694441,2.096462154,,13193.92327,517769.9149,,13191.84,517772.32
23 | -2.445570082,0.280922506,,13188.99272,517770.235,,13189.09,517772.53
24 | -2.18820077,1.511473596,,13186.84336,517771.6682,,13186.65,517773.13
25 | -1.670585801,1.207795114,,13184.16498,517775.4645,,13184.79,517773.86
26 | -1.346962366,0.739544394,,13184.00931,517772.9916,,13183.07,517774.65
27 | -0.871990945,1.26533171,,13180.6505,517773.8957,,13181.94,517775.69
28 | -0.554885537,0.84231037,,13183.34785,517777.8212,,13181.24,517776.52
29 | -0.317779521,1.016938074,,13179.75717,517777.3872,,13180.98,517777.55
30 | 0.691059342,1.146278735,,13183.64684,517779.0771,,13181.16,517778.82
31 | 2.008524405,0.527229064,,13181.16652,517780.0134,,13182.5,517779.91
32 | 2.506278185,1.542810203,,13184.58733,517780.5599,,13185.15,517781.48
33 | 1.73233612,0.574587356,,13187.38633,517784.7035,,13186.2,517782.62
34 | 1.145225359,0.989280962,,13188.58543,517785.1316,,13187.25,517783.57
35 | 1.585298999,0.802694471,,13190.82738,517786.8997,,13188.58,517784.51
36 | 1.854568327,0.103825182,,13191.95676,517787.0367,,13190.34,517785.26
37 | 1.507901898,0.71857473,,13191.6213,517786.0196,,13192.1,517785.95
38 | 2.91383482,-0.400100341,,13194.1487,517784.9372,,13194.43,517786.57
39 | 2.461352953,0.487836662,,13194.20159,517789.098,,13196.63,517787.31
40 | 2.292480193,0.44906072,,13200.50343,517788.4357,,13198.97,517788.12
41 | 1.721152914,0.225694292,,13199.5511,517788.1756,,13201.01,517788.56
42 | 2.430063263,-0.210575686,,13204.60954,517787.2261,,13203.05,517788.73
43 | 2.178333617,0.181224609,,13206.79243,517787.0981,,13205.08,517788.96
44 | 2.062321277,-0.015028717,,13206.47526,517788.9386,,13207.26,517789.06
45 | 1.994784013,-0.362050867,,13210.80359,517787.9084,,13209.29,517789.04
46 | 1.577737739,-0.636040415,,13210.55057,517790.9046,,13211.17,517788.96
47 | 1.555374417,-0.270415735,,13214.53883,517787.0523,,13212.76,517788.95
48 | 1.412734827,-0.208455414,,13214.60969,517791.1926,,13213.92,517788.88
49 | 0.356454587,-0.017946969,,13216.28235,517789.6955,,13214.36,517788.87
50 | 0.280151346,0.170127322,,13214.6179,517789.4177,,13214.51,517789.12
51 | -0.012250119,0.526208936,,13214.78546,517788.1185,,13214.38,517789.63
52 | -1.540647005,0.894917039,,13211.04518,517787.8441,,13212.79,517789.9
53 | -3.819673477,0.47576835,,13209.87539,517787.8555,,13209.47,517790.13
54 | -2.088783281,2.530731772,,13206.69924,517793.4151,,13207.06,517792.07
55 | -2.198604566,1.61847168,,13205.59703,517794.8087,,13205.22,517793.62
56 | -1.695641004,0.638837461,,13205.54286,517792.3996,,13203.64,517794.09
57 | -1.661979875,4.560674984,,13203.32052,517795.8844,,13200.85,517797.97
58 | -1.43769479,2.561183232,,13196.15087,517801.9986,,13198.44,517799.9
59 | -1.32795207,2.05013819,,13198.52905,517802.2655,,13196.46,517801.52
60 | -0.803800567,2.443791101,,13195.28421,517802.7377,,13194.63,517803.4
61 | -1.121617283,1.79865308,,13195.97933,517803.797,,13193.53,517805.14
62 | -1.042916372,1.790592124,,13194.58064,517808.3911,,13192.56,517806.87
63 | 0.042755687,1.874978392,,13194.39847,517808.8612,,13192.32,517808.73
64 | 0.276199219,1.933785782,,13193.32248,517812.977,,13192.67,517810.65
65 | 1.917535177,1.456999623,,13195.69015,517812.7393,,13193.74,517812.49
66 | 2.562949332,1.450823088,,13195.86991,517813.8519,,13196.4,517814.13
67 | 1.385808777,0.660799046,,13198.12784,517812.8707,,13197.87,517815.07
68 | 1.81443682,-0.365648727,,13200.69932,517813.7154,,13199.62,517815.33
69 | 1.04104898,-0.854227672,,13203.48595,517816.4715,,13201.07,517814.9
70 | 1.239795657,-0.636754832,,13203.42086,517817.1235,,13202.37,517814.81
71 | 0.931906959,-0.626316667,,13204.36854,517813.21,,13203.52,517814.55
72 | 0.668824242,-1.5257656,,13204.17876,517813.1979,,13204.2,517813.01
73 | 0.862193417,-0.110824227,,13206.01111,517813.7477,,13205.06,517813.09
74 | 0.06610085,0.826721187,,13207.13692,517815.4814,,13205.09,517813.92
75 | -2.255859495,-2.622971232,,13205.35396,517812.0035,,13203.83,517810.48
76 | -2.176554462,1.738852812,,13200.75835,517811.9754,,13200.66,517811.28
77 | -1.725643407,-1.179725915,,13201.06605,517812.1106,,13198.6,517810.02
78 | -1.421297057,1.037627097,,13195.0172,517811.9749,,13196.73,517810.67
79 | -1.892865807,0.079365079,,13193.10734,517811.2001,,13194.83,517810.05
80 | -2.102840453,-0.025414512,,13191.60522,517808.4514,,13193.08,517809.62
81 | -1.70232498,-0.365564085,,13189.27556,517808.105,,13191.62,517809.31
82 | -1.464439133,0.225164348,,13192.19757,517806.9739,,13189.88,517809.26
83 | -1.574496935,1.177489784,,13186.52159,517811.5645,,13188.02,517810.05
84 | -1.403743138,1.66705943,,13184.09617,517812.693,,13186.16,517811.16
85 | -1.352843829,1.444872878,,13186.7698,517810.751,,13184.45,517812.13
86 | -1.091307957,1.079545974,,13182.27989,517811.7325,,13183.02,517812.85
87 | -0.870796477,3.731409415,,13181.00064,517817.9021,,13180.79,517815.95
88 | -1.684912389,1.400345903,,13179.70985,517815.9754,,13178.65,517816.87
89 | -1.254554441,2.596673808,,13177.43759,517821.4126,,13176.97,517819.12
90 | -1.756721278,1.915035926,,13176.4752,517821.3957,,13175.43,517820.93
91 | -0.517595923,2.002550656,,13171.72975,517822.3641,,13174.03,517822.48
92 | -0.4096,1.881858349,,13175.12806,517821.9698,,13172.77,517823.97
93 | -0.266771872,1.339752263,,13171.12018,517825.0972,,13172.52,517825.32
94 | -0.187108843,1.502317919,,13173.83141,517828.1105,,13171.98,517826.73
95 | -0.09219082,1.215956815,,13172.27417,517828.821,,13171.43,517827.82
96 | 0.371099841,0.250358378,,13171.38395,517826.783,,13171.87,517828.13
97 | -0.10581856,0.847116329,,13171.6368,517830.5803,,13171.75,517828.98
98 | 


--------------------------------------------------------------------------------
/src/pedometer/StepDetector.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/pedometer/StepDetector.java


--------------------------------------------------------------------------------
/src/pedometer/acc.txt:
--------------------------------------------------------------------------------
  1 | 0.21667525 2.2661119 8.93935
  2 | 0.47524905 2.280477 9.274539
  3 | 0.676362 2.0697873 9.288904
  4 | 0.22625206 2.112883 9.513959
  5 | 0.31244332 1.9979613 9.772532
  6 | 0.2741361 2.136825 9.365519
  7 | 0.14963761 2.0841527 9.264962
  8 | 0.130484 1.9405005 9.250597
  9 | 0.33638534 1.9644425 9.346365
 10 | 0.39863458 2.03148 9.159617
 11 | 0.2789245 2.0266917 9.264962
 12 | 0.30765492 1.9500773 9.882666
 13 | 0.37948096 2.0219033 9.7007065
 14 | 0.29807812 1.9644425 9.619304
 15 | 0.16400282 1.8734628 9.609727
 16 | 0.073023155 1.7633295 9.466075
 17 | 0.25498247 1.7202339 9.686341
 18 | 0.2885013 1.8590976 9.552266
 19 | 0.3890578 1.9309237 9.399037
 20 | 0.36032736 1.9596541 9.394249
 21 | 0.15442601 1.8255788 9.5570545
 22 | -0.051475335 1.638831 9.643246
 23 | 0.130484 1.7776948 9.403826
 24 | 0.092176765 1.7776948 9.341577
 25 | -0.24779987 1.590947 9.260174
 26 | -0.089782566 1.5382746 9.57142
 27 | -0.17118542 1.5334862 9.648034
 28 | -0.108936176 1.5191209 9.533113
 29 | 0.11133038 1.6484078 9.7007065
 30 | -0.094570965 1.4856021 9.796474
 31 | -0.11372458 1.34195 9.925761
 32 | -0.003591303 1.4185646 9.743802
 33 | -0.003591303 1.471237 9.513959
 34 | 0.10175357 1.4616601 9.470863
 35 | 0.1352724 1.4664485 9.485229
 36 | 0.20709845 1.4425066 9.695918
 37 | 0.37948096 1.6053122 9.6049385
 38 | 0.25977087 1.5095441 9.686341
 39 | 0.39863458 1.5382746 9.715072
 40 | 0.49919105 1.6962919 9.638457
 41 | 0.427365 1.8064252 9.389461
 42 | 0.32202014 1.7824832 9.221867
 43 | 0.015562311 1.543063 9.312846
 44 | -0.20470424 1.3371617 9.5570545
 45 | -0.16160862 1.3563153 9.336788
 46 | -0.12808979 1.4329298 9.221867
 47 | 0.16879122 1.614889 9.427768
 48 | 0.077811554 1.6675615 9.456498
 49 | 0.034715924 1.7106571 9.389461
 50 | 0.039504327 1.8590976 9.169194
 51 | 0.25498247 2.03148 9.159617
 52 | 0.36032736 1.9452889 9.585785
 53 | 0.11133038 1.8782512 9.930551
 54 | -0.061052144 1.887828 10.02153
 55 | -0.003591303 2.0410569 9.609727
 56 | -0.08499416 2.184709 8.982447
 57 | -0.25737667 1.9117701 9.178771
 58 | -0.24779987 1.7729063 9.887454
 59 | -0.142455 1.7968484 9.844358
 60 | -0.07062895 1.863886 9.442133
 61 | -0.008379706 1.8734628 9.078215
 62 | -0.07062895 1.7776948 9.12131
 63 | -0.19512744 1.6915035 8.987235
 64 | 0.049081136 1.7633295 9.159617
 65 | 0.5710171 1.8590976 9.916184
 66 | 0.78170687 1.9500773 10.356718
 67 | 0.700304 2.1799207 10.23222
 68 | 0.8631097 2.4480712 9.825205
 69 | 0.9684546 2.6252422 9.782109
 70 | 1.0307038 2.634819 10.035895
 71 | 0.8726865 2.586935 9.801263
 72 | 0.7290344 2.5438392 9.327211
 73 | 0.6572084 2.3427265 9.068638
 74 | 0.7290344 2.208651 9.073426
 75 | 0.8678981 2.1511903 8.958505
 76 | 0.9780314 2.2182279 9.140464
 77 | 0.8295909 2.3666685 8.963293
 78 | 0.80086046 2.586935 8.800487
 79 | 0.7721301 2.658761 9.140464
 80 | 0.82001406 2.7449522 9.145252
 81 | 0.9588778 2.7976246 9.375095
 82 | 0.8678981 2.7832594 9.183559
 83 | 0.8535329 2.8072014 8.891466
 84 | 0.80564886 2.8119898 8.81964
 85 | 0.6667852 2.7449522 8.589797
 86 | 0.5710171 2.778471 8.565855
 87 | 0.29328972 2.7688942 8.613739
 88 | 0.092176765 2.802413 8.743026
 89 | 0.029927522 2.9460652 8.886678
 90 | -0.05626374 3.1663318 9.054273
 91 | -0.003591303 3.4823663 9.327211
 92 | 0.015562311 3.7600937 9.681553
 93 | -0.17597382 3.6643257 10.50037
 94 | -0.44891283 3.357868 10.955269
 95 | -0.712275 3.228581 10.897807
 96 | -0.6452374 3.2860417 10.825981
 97 | -0.7314286 3.965995 9.968858
 98 | -0.027533319 4.3155484 7.459734
 99 | 0.3890578 2.9508536 8.245032
100 | -0.094570965 0.3890578 12.975975
101 | -1.0953473 -1.8423382 16.576855
102 | -3.1639376 -1.6124948 15.523405
103 | -1.3251907 1.2270284 10.452486
104 | -0.31004912 3.1615434 5.9370217
105 | -0.6500258 2.7641058 3.5045128
106 | -1.8423382 1.447295 4.4957123
107 | -2.1392193 0.2645593 6.5882444
108 | -1.6843209 -0.18076223 8.585009
109 | -0.93254155 -0.10414778 9.897031
110 | -0.6356605 0.21188685 10.945692
111 | -1.1288661 0.45130703 11.462839
112 | -1.7609353 1.0498575 10.347141
113 | -1.4879963 2.112883 8.834005
114 | -0.81283146 2.9412768 8.278551
115 | -0.89902276 3.1950622 8.292916
116 | -1.1336545 3.0609868 8.139688
117 | -1.1575965 2.9700072 8.010401
118 | -1.555034 2.4768016 8.422203
119 | -1.7753005 2.12246 8.685565
120 | -1.5406688 2.0649989 9.169194
121 | -1.5167267 2.1080947 9.518747
122 | -1.411382 2.2852654 9.226655
123 | -1.1049241 2.6300306 8.757391
124 | -0.9947908 2.9173348 8.398261
125 | -1.1288661 3.0226796 8.3312235
126 | -1.1575965 3.0657754 8.383896
127 | -0.961272 3.252523 8.58022
128 | -0.6739678 3.4344823 8.934562
129 | -0.48243165 3.6212301 9.274539
130 | -0.48243165 3.855862 9.346365
131 | -0.39145198 4.0138793 9.595362
132 | -0.37708676 4.119224 10.050261
133 | -0.4201824 3.9468415 10.495582
134 | -0.51116204 3.6020765 11.060614
135 | -0.7649474 3.6164417 10.912173
136 | 0.1448492 3.515885 9.537901
137 | 0.8439561 3.0418332 9.351153
138 | 1.6244658 1.0594343 12.09012
139 | 0.973243 -0.8367735 16.845005
140 | 0.025139118 -1.4496891 16.495451
141 | -0.33399114 -0.061052144 10.323199
142 | -0.08499416 1.1599907 5.4964886
143 | 0.70509243 1.4568717 3.3321302
144 | 2.2373815 1.5191209 5.2474914
145 | 2.6156654 1.0594343 8.345589
146 | 1.9452889 0.81043726 10.548254
147 | 0.6715736 0.79607207 10.572196
148 | 0.029927522 1.1360487 8.829217
149 | -0.08020575 1.5622166 7.459734
150 | 0.029927522 1.9357121 7.402273
151 | 0.11611878 1.79206 8.422203
152 | 0.33638534 1.6867151 9.073426
153 | 0.6955156 1.6819267 9.485229
154 | 0.91099375 1.7441759 9.705495
155 | 0.9828198 1.9835961 9.504382
156 | 1.0594343 2.3187842 9.111733
157 | 1.1025299 2.634819 9.221867
158 | 1.1025299 2.9652188 9.164406
159 | 1.0929531 3.3722332 9.0016
160 | 0.8487445 3.3913867 9.327211
161 | 0.34117374 3.0945058 9.231443
162 | 0.14963761 2.8933928 9.21229
163 | 0.06823475 3.027468 8.857947
164 | 0.1592144 3.2716765 8.79091
165 | 0.005985504 3.3530796 9.111733
166 | -0.15682021 3.453636 9.542689
167 | -0.25258827 3.5637693 10.131663
168 | -0.12330139 3.6882677 10.802039
169 | -0.22385786 3.6739025 10.720636
170 | -0.88944596 3.558981 10.332776
171 | -1.6843209 3.333926 10.476428
172 | -1.2150574 4.368221 9.863512
173 | 0.049081136 5.043386 7.2107368
174 | 1.6675615 3.678691 9.882666
175 | 0.49919105 0.7529764 14.762049
176 | -2.3834279 -1.6795325 15.671845
177 | -3.930082 -0.45848963 11.180324
178 | -0.6548142 2.6731262 7.4932528
179 | 0.20709845 3.1615434 6.669647
180 | -1.7657237 1.423353 7.766192
181 | -2.292448 0.5231331 8.455722
182 | -1.6986861 0.61411273 8.680777
183 | -0.89902276 1.4041992 8.3072815
184 | -0.51595044 2.0410569 7.814076
185 | -0.37708676 2.5582044 7.2203135
186 | -0.79367787 2.4672248 7.330447
187 | -1.282095 2.1511903 7.5171947
188 | -1.0522517 2.1416135 8.225879
189 | -0.49679685 2.505532 8.848371
190 | -0.17118542 3.132813 8.838794
191 | -0.6835446 3.05141 8.522759
192 | -1.2677298 2.8550856 8.422203
193 | -1.3587095 2.8550856 8.623316
194 | -1.0666169 3.0705638 8.407838
195 | -0.6548142 3.4919431 8.556278
196 | -0.52073884 3.846285 8.441357
197 | -0.5590461 3.8606503 8.225879
198 | -0.6404489 3.807978 8.407838
199 | -0.45848963 3.7792473 9.12131
200 | -0.49200845 3.8510735 9.6288805
201 | -0.45370123 3.7984009 10.337564
202 | -0.2765303 3.6595373 11.156382
203 | -0.45370123 3.3147724 11.386225
204 | -0.49200845 3.0322564 10.610503
205 | -0.49200845 3.4009635 10.600926
206 | 0.6380547 3.3291376 9.327211
207 | 2.088941 2.9125464 10.634445
208 | 2.6539726 1.4616601 14.565724
209 | 1.2270284 -0.12330139 15.906478
210 | 0.34596214 -0.4201824 12.932879
211 | 0.12090719 0.18794483 8.082227
212 | 0.9780314 0.78649527 6.8324533
213 | 2.1990743 0.90141696 8.728661
214 | 2.1894975 1.0785879 9.9928
215 | 1.887828 1.9117701 9.6049385
216 | 1.3850456 2.5246856 8.039131
217 | 0.7577648 2.8072014 6.487688
218 | 0.15442601 2.5007436 5.7215433
219 | 0.08259996 2.1894975 5.9801173
220 | 0.2741361 1.8064252 7.5985975
221 | 0.22146365 1.2509704 9.025542
222 | 0.14963761 0.94930094 9.892242
223 | 0.29807812 1.0642227 10.165182
224 | 0.2789245 1.6819267 9.6049385
225 | 0.34117374 2.280477 8.857947
226 | 0.748188 2.9173348 8.446145
227 | 1.1456255 3.405752 8.589797
228 | 1.1456255 3.678691 8.613739
229 | 0.93014735 3.8271315 8.436568
230 | 0.1352724 3.716998 8.465299
231 | 0.049081136 3.3674448 8.297705
232 | -0.075417355 3.0657754 8.455722
233 | -0.5351041 3.0945058 8.992023
234 | -0.5590461 3.2381577 9.442133
235 | -0.5734113 3.6164417 9.576208
236 | -0.30526072 3.6355953 10.270527
237 | 0.36511576 3.9324763 11.577761
238 | 0.1592144 3.9516299 12.003929
239 | -0.7553706 3.5637693 11.654375
240 | -1.459266 3.630807 10.782886
241 | -0.93254155 5.0002904 8.446145
242 | -0.14724341 5.019444 8.16363
243 | 0.5231331 1.9452889 11.893796
244 | -0.06584055 -0.7553706 15.264832
245 | -2.2254105 -1.8614918 13.790003
246 | -2.373851 0.34596214 8.867524
247 | -0.18076223 3.0609868 5.324106
248 | 0.020350715 3.9755719 4.859631
249 | -1.3204023 1.5765818 6.7749925
250 | -2.0721815 0.51355624 8.154053
251 | -1.8327614 0.077811554 9.193136
252 | -0.9756372 0.6859388 9.494805
253 | 0.16400282 1.7633295 9.312846
254 | 0.19273324 1.9165585 9.097368
255 | -0.961272 1.6819267 8.733449
256 | -1.2485762 1.7441759 7.8667483
257 | -0.664391 2.5151088 7.555502
258 | -0.20949265 3.0897174 8.144476
259 | -0.25737667 3.477578 9.025542
260 | -0.7026982 3.2094274 8.924985
261 | -1.5358804 2.5486276 8.46051
262 | -1.5071499 2.337938 8.556278
263 | -0.961272 2.48159 8.58022
264 | -0.93254155 2.5582044 8.968081
265 | -0.9947908 2.754529 9.178771
266 | -0.89902276 3.1950622 8.853159
267 | -0.58298814 3.654749 8.345589
268 | -0.7457938 3.702633 8.479664
269 | -0.82240826 3.4727895 9.264962
270 | -0.7457938 3.3530796 10.356718
271 | -0.6452374 3.1998506 11.261726
272 | -0.58298814 3.084929 11.821969
273 | -0.21428105 3.1615434 11.673529
274 | 0.092176765 3.3770216 10.883442
275 | 0.17357962 3.5972881 10.179547
276 | 0.5566519 3.0131028 9.528324
277 | 2.055422 2.2182279 11.663952
278 | 2.3810337 1.0067618 14.944009
279 | 1.174356 -0.63087213 15.05893
280 | -0.5303157 -0.9660604 11.04146
281 | 0.1448492 -0.36750996 7.6847887
282 | 1.5765818 0.2741361 7.5171947
283 | 2.5103204 0.49919105 9.509171
284 | 1.7154455 0.7146692 10.117298
285 | 1.2844892 1.5717934 8.709507
286 | 0.70509243 2.1320367 7.340024
287 | 0.17357962 2.3666685 6.1524997
288 | -0.017956512 2.2134395 5.5922565
289 | 0.039504327 1.9931729 6.1141925
290 | 0.23104046 1.5765818 8.048708
291 | 0.2837129 1.1216835 9.69113
292 | 0.08738836 0.93493575 10.26095
293 | 0.049081136 1.34195 9.6528225
294 | 0.25019407 1.9500773 8.968081
295 | 0.51355624 2.5773582 8.168418
296 | 0.81522566 3.0418332 7.838018
297 | 0.9588778 3.4680011 7.814076
298 | 0.82480246 3.6116533 8.096592
299 | 0.46567222 3.5350387 8.647258
300 | 0.29807812 3.4680011 8.479664
301 | 0.17357962 3.4249055 8.465299
302 | 0.11611878 3.3722332 9.015965
303 | -0.2621651 3.0561984 9.437345
304 | -0.2669535 3.0945058 9.758167
305 | -0.2621651 3.108871 10.160394
306 | -0.1376666 3.2381577 11.314399
307 | -0.18076223 3.4249055 12.415731
308 | -0.6931214 3.7361517 12.521076
309 | -0.7266402 4.5501804 10.940904
310 | -0.89423436 4.0904937 9.681553
311 | -0.027533319 2.0458453 12.602479
312 | 0.6715736 0.29328972 15.968727
313 | -1.7609353 -1.1480197 15.173852
314 | -2.8670566 0.5614403 9.796474
315 | -0.61650693 3.2573113 6.3344593
316 | 0.59974754 3.4584243 5.94181
317 | -1.5837644 1.4760253 7.3687544
318 | -2.1871033 0.2837129 7.86196
319 | -1.6077064 0.1448492 8.158841
320 | -0.7553706 0.6955156 8.029554
321 | -0.5686229 1.1456255 7.1101804
322 | -0.59735334 1.3994108 6.976105
323 | -0.58298814 1.4425066 7.450157
324 | -0.5494693 1.4616601 7.8954787
325 | -0.38187516 1.6579847 8.067862
326 | -0.25258827 2.0362685 8.010401
327 | -0.47764325 2.6779146 7.972093
328 | -0.4345476 3.228581 7.9912467
329 | -0.62608373 3.4680011 8.245032
330 | -0.60693014 3.7074213 8.752603
331 | -0.6835446 3.558981 9.044696
332 | -0.92296475 3.4871547 9.375095
333 | -0.8655039 3.7313633 9.015965
334 | -0.7697359 3.8654387 8.987235
335 | -0.94690675 3.630807 9.308058
336 | -1.1528081 3.2573113 9.671976
337 | -1.1192893 2.8502972 10.361506
338 | -0.7697359 2.48159 11.314399
339 | -0.20470424 2.3475149 12.410943
340 | 0.16400282 2.5390508 12.573749
341 | 0.33159694 2.8646624 11.87943
342 | 0.91578215 3.1519666 9.983223
343 | 1.8590976 2.8646624 9.729437
344 | 2.8072014 0.9923966 14.584878
345 | 1.1360487 -1.5310919 16.998234
346 | -0.2908955 -1.4257472 12.746131
347 | 0.5614403 -0.24301147 7.732673
348 | 2.0841527 0.2837129 8.829217
349 | 2.553416 0.11611878 11.237784
350 | 1.7250223 -0.108936176 11.271303
351 | 1.1504139 0.58538234 9.351153
352 | 1.2270284 1.6627731 6.8611836
353 | 1.3036429 2.6013002 4.8644195
354 | 1.1647792 2.9891608 3.6050692
355 | 0.48003745 2.8072014 3.9594111
356 | 0.427365 2.7210102 5.065532
357 | 0.59495914 2.3666685 6.4828997
358 | 0.5374983 1.8255788 8.177995
359 | 0.45609543 1.0642227 10.184336
360 | 0.034715924 0.6955156 11.223419
361 | -0.29568392 1.0737995 10.773309
362 | 0.092176765 1.9500773 9.547478
363 | 0.4369418 2.7162218 8.613739
364 | 0.65242 3.2190042 8.139688
365 | 0.90620536 3.7217865 8.039131
366 | 0.6619968 3.8989575 7.814076
367 | 0.21188685 3.764882 7.9529395
368 | -0.022744916 3.726575 8.46051
369 | -0.30047232 3.654749 9.078215
370 | -0.5494693 3.3147724 10.457274
371 | -0.79367787 2.9317 12.334329
372 | -1.009156 2.586935 13.445238
373 | -1.1384429 3.1040826 12.588114
374 | -0.7266402 4.176685 10.634445
375 | -0.49679685 4.056975 9.274539
376 | 0.3172317 2.1990743 12.80838
377 | 0.40821138 0.10175357 16.91683
378 | -0.961272 -1.3922282 15.719729
379 | -1.5023615 0.7194576 9.796474
380 | 0.1352724 2.8742392 5.319318
381 | 0.45130703 2.9269116 5.707178
382 | -0.90859956 1.1121067 8.125322
383 | -1.1959038 0.20231004 9.145252
384 | -1.0522517 0.19752164 8.446145
385 | -0.61171854 0.80086046 7.3208704
386 | -0.51116204 1.0642227 6.851607
387 | -0.2621651 1.2605472 6.8563952
388 | -0.37229836 1.3802572 7.114969
389 | -0.47285482 1.4377182 7.579444
390 | -0.49679685 1.5861586 7.65127
391 | -0.2765303 1.816002 7.7997108
392 | -0.01316811 2.2038627 7.933786
393 | 0.06344634 2.6491842 8.182783
394 | -0.008379706 3.204639 8.254609
395 | -0.48243165 3.3435028 8.714295
396 | -0.61650693 3.4823663 9.202713
397 | -0.35793316 3.750517 9.791686
398 | -0.17118542 3.9803603 9.873089
399 | -0.2669535 4.0043025 9.312846
400 | -0.12808979 3.750517 9.983223
401 | -0.24779987 2.9460652 11.778873
402 | -0.7649474 2.2948422 13.047801
403 | -0.5398925 2.4432828 12.664728
404 | -0.20470424 2.7976246 12.228984
405 | -0.17118542 3.5206735 10.442909
406 | 0.91578215 3.879804 8.096592
407 | 2.328361 1.0259154 12.80838
408 | 1.8590976 -2.086547 18.96148
409 | -0.003591303 -3.369839 16.663046
410 | 0.025139118 -1.1049241 9.547478
411 | 1.6627731 1.0977415 6.2243257
412 | 2.8742392 1.246182 8.245032
413 | 1.8974048 0.79607207 9.264962
414 | 0.9684546 1.0402806 8.556278
415 | 0.8822633 1.5574282 7.8715367
416 | 0.7769185 1.9740193 6.497265
417 | 0.7146692 2.4193408 5.0559554
418 | 0.61411273 2.8790276 4.2515035
419 | 0.40821138 2.9460652 5.084686
420 | 0.21667525 2.5677812 6.2243257
421 | 0.20231004 2.0841527 7.4166384
422 | 0.29328972 1.5957354 9.466075
423 | 0.05865794 1.2557588 11.084556
424 | -0.45848963 1.5382746 11.017518
425 | -0.18076223 2.2517467 9.442133
426 | 0.24061728 3.1232362 8.517971
427 | 0.48961425 3.654749 8.130111
428 | 0.33159694 3.8893807 7.603386
429 | -0.07062895 3.9516299 7.6225395
430 | -0.51595044 3.7696705 7.7709804
431 | -0.6691794 3.7792473 8.575432
432 | -0.81761986 3.5110967 10.2417965
433 | -1.3587095 2.9700072 11.683105
434 | -1.5741876 2.634819 13.196241
435 | -1.5358804 2.9843724 13.253702
436 | -2.3116016 4.2341456 11.256938
437 | -0.7266402 5.2061915 8.968081
438 | 0.7433996 2.7736826 12.477981
439 | 0.12569559 0.23104046 15.963939
440 | -1.9812019 -1.1671734 13.335105
441 | -1.9285295 0.4369418 7.6129627
442 | -0.51595044 2.5103204 5.324106
443 | 0.37469256 2.4863784 7.8332295
444 | -1.0235212 1.198298 9.284116
445 | -1.3251907 0.45130703 9.767744
446 | -1.0666169 0.7290344 8.905831
447 | -0.79846627 1.3563153 7.354389
448 | -0.51116204 1.9692309 6.4062853
449 | -0.38187516 2.2948422 5.9561753
450 | -0.23343466 2.3953989 6.2769985
451 | -0.051475335 2.2852654 7.474099
452 | -0.022744916 2.1942859 8.738237
453 | 0.025139118 2.1751323 9.178771
454 | -0.4297592 2.3331497 9.169194
455 | -0.35793316 2.9173348 8.992023
456 | -0.47764325 3.2381577 8.206725
457 | -0.6452374 3.429694 8.249821
458 | -0.25258827 3.669114 8.752603
459 | -0.12808979 3.8367083 8.81964
460 | -0.11372458 3.8989575 8.766968
461 | -0.17118542 3.894169 8.977658
462 | -0.1376666 4.047398 9.049484
463 | -0.07062895 4.2437224 9.466075
464 | -0.30047232 3.8845923 10.2896805
465 | -0.075417355 3.5781345 11.66874
466 | 0.23582886 3.204639 12.612056
467 | 0.22625206 3.05141 12.219407
468 | 0.06344634 3.645172 10.64881
469 | 0.700304 3.4871547 6.937798
470 | 1.9213469 2.0171149 9.777321
471 | 2.0745757 -0.4249708 15.461156
472 | 0.47046062 -2.110489 16.993444
473 | -0.20470424 -1.1049241 12.511499
474 | 0.8678981 0.36990416 8.326435
475 | 2.2421699 1.1599907 8.2354555
476 | 2.6922798 1.1935096 9.384672
477 | 2.1033063 1.6484078 8.393473
478 | 1.5382746 2.2038627 7.2107368
479 | 0.7098808 2.4241292 5.9801173
480 | 0.11611878 2.3953989 5.017648
481 | 0.2741361 2.256535 5.486912
482 | 0.4369418 1.9596541 7.4549456
483 | 0.47046062 1.6292542 9.025542
484 | 0.6907272 1.2605472 10.883442
485 | 0.39384618 0.9923966 11.908161
486 | 0.11133038 1.1791444 11.323976
487 | 0.37469256 2.0458453 8.628104
488 | 0.7577648 2.8311436 7.483676
489 | 0.7673417 3.027468 7.713519
490 | 0.7098808 3.1615434 8.154053
491 | 0.9828198 3.5350387 8.22109
492 | 1.0019734 3.9995139 8.024766
493 | 0.5566519 3.9564183 7.876325
494 | 0.23104046 3.8510735 8.278551
495 | -0.022744916 3.6643257 9.231443
496 | -0.2908955 3.4919431 9.877877
497 | -0.48722005 3.3147724 10.864288
498 | -0.7074866 3.1711202 11.735778
499 | -0.94690675 3.1663318 12.123639
500 | -1.2916719 3.8031895 11.764508
501 | -0.9708488 4.64116 9.490017
502 | -0.08020575 3.894169 9.705495
503 | 0.18315643 1.4425066 14.287997
504 | -0.80804306 -0.81761986 15.916055
505 | -2.349909 -0.3866636 10.2417965
506 | -1.2150574 2.304419 5.04159
507 | 1.021127 4.1718965 4.5675383
508 | -0.14724341 2.3570917 8.3312235
509 | -1.4544775 0.6667852 10.385448
510 | -1.6124948 0.1592144 10.797251
511 | -0.93254155 0.81522566 9.561843
512 | -0.03232172 1.6771383 8.149264
513 | -0.046686932 2.098518 7.177218
514 | -0.21906945 2.2182279 6.4924765
515 | -0.40102878 2.4528596 5.9705405
516 | -0.22385786 2.6491842 6.296152
517 | -0.003591303 2.6779146 7.306505
518 | -0.017956512 2.5246856 8.096592
519 | -0.1376666 2.7401638 8.623316
520 | -0.3196259 2.883816 8.891466
521 | -0.6404489 2.9460652 8.824429
522 | -0.44891283 3.357868 8.532336
523 | -0.037110128 4.0186677 8.450933
524 | -0.10414778 4.2676644 8.450933
525 | -0.23822308 4.2916064 8.6712
526 | -0.25737667 4.0043025 9.470863
527 | -0.415394 3.558981 10.423756
528 | -0.20470424 3.156755 11.6352215
529 | 0.12569559 3.0178912 13.086108
530 | 0.34596214 3.0370448 12.918514
531 | -0.2861071 3.4727895 11.6352215
532 | 0.62368953 3.38181 7.86196
533 | 1.3467385 2.2230163 8.3551655
534 | 2.1511903 -0.36750996 14.465168
535 | 0.7146692 -2.7138276 18.698116
536 | -0.79846627 -1.9907787 13.775638
537 | 0.59974754 0.37469256 7.4310036
538 | 2.3570917 1.8543092 5.8843493
539 | 3.38181 1.3611037 9.8683
540 | 2.3570917 0.62847793 11.596914
541 | 1.423353 1.0115502 10.83077
542 | 1.1935096 2.1416135 7.7949224
543 | 0.8439561 2.8119898 5.357625
544 | 0.65242 3.0705638 3.9785647
545 | 0.5231331 2.6683378 4.299388
546 | 0.58538234 2.1272483 6.4445925
547 | 0.6380547 1.543063 8.522759
548 | 0.7098808 1.021127 10.318411
549 | 0.31244332 0.94930094 11.051037
550 | -0.2765303 1.1216835 10.1939125
551 | -0.36750996 1.638831 8.675988
552 | 0.077811554 2.36188 8.029554
553 | 0.61411273 2.9125464 8.417415
554 | 0.8822633 3.5063083 8.398261
555 | 0.78649527 3.894169 8.154053
556 | 0.50397944 4.2054152 8.19236
557 | 0.36990416 4.2341456 8.455722
558 | 0.21188685 4.0282445 9.245809
559 | -0.09935937 3.6978445 10.184336
560 | -0.44891283 3.309984 11.242573
561 | -0.79846627 3.1232362 12.30081
562 | -1.0139444 3.3195608 12.2241955
563 | -1.2246342 3.8989575 11.625645
564 | -0.8798691 4.985925 9.427768
565 | 0.09696517 4.8662148 8.513183
566 | 0.6715736 1.9692309 12.794015
567 | -0.008379706 -0.7410054 16.188993
568 | -2.1440077 -1.7130513 12.679093
569 | -1.6651673 0.5422867 7.1101804
570 | 0.30286652 2.8933928 4.869208
571 | 0.49440265 2.3762453 7.20116
572 | -0.6931214 0.9828198 8.848371
573 | -0.9516952 0.32680854 9.638457
574 | -0.8559271 0.6572084 8.795698
575 | -0.2908955 1.5191209 7.2729864
576 | -0.1328782 1.9069817 6.4541693
577 | 0.039504327 2.1942859 6.5307837
578 | -0.046686932 2.0937295 7.569867
579 | -0.2621651 2.0123265 8.091804
580 | -0.23822308 2.0362685 8.628104
581 | -0.2861071 2.409764 9.025542
582 | -0.37229836 3.0226796 8.824429
583 | -0.5781997 3.2908301 8.197148
584 | -0.6787562 3.2477345 8.494029
585 | -0.7457938 3.2333694 8.752603
586 | -0.49679685 3.5446155 9.102157
587 | -0.32441434 3.8654387 9.111733
588 | -0.32920274 4.0904937 8.987235
589 | -0.30047232 4.1240125 8.757391
590 | -0.2813187 3.965995 9.197925
591 | -0.52073884 3.1854854 10.50037
592 | -0.45370123 2.7257986 11.845911
593 | 0.10654198 2.7497406 12.707824
594 | 0.21188685 3.027468 11.989564
595 | 0.2693477 3.6164417 10.174759
596 | 1.3323733 3.38181 8.417415
597 | 2.6779146 1.863886 11.932103
598 | 1.9548657 -0.061052144 16.083649
599 | 0.17357962 -1.3395559 15.801132
600 | -0.2717419 -0.24779987 10.002377
601 | 0.5422867 0.700304 6.0088477
602 | 2.2421699 1.34195 7.057508
603 | 3.0992942 0.9780314 10.208278
604 | 1.9213469 0.5470751 11.730989
605 | 0.7625532 0.9876082 10.524312
606 | 0.6428431 2.2182279 7.603386
607 | 0.47046062 2.658761 5.644929
608 | 0.37469256 2.6491842 5.084686
609 | 0.6332663 2.280477 5.769428
610 | 0.8487445 1.8064252 7.105392
611 | 0.9876082 1.5861586 8.474875
612 | 1.0067618 1.6292542 9.015965
613 | 0.7098808 1.7298107 9.422979
614 | 0.4369418 2.1080947 9.015965
615 | 0.47046062 2.5821466 8.254609
616 | 0.7721301 3.0466216 8.091804
617 | 0.9876082 3.5206735 8.249821
618 | 0.8487445 3.822343 8.158841
619 | 0.6667852 3.927688 8.359954
620 | 0.06823475 3.678691 8.953716
621 | -0.40581718 3.357868 9.403826
622 | -0.5542577 3.0992942 10.275315
623 | -0.4201824 2.883816 11.730989
624 | -0.2621651 3.0609868 13.421296
625 | -0.62129533 4.238934 12.616844
626 | 0.073023155 5.2444987 9.097368
627 | 0.51355624 3.4584243 10.821193
628 | 0.09696517 1.0929531 15.020623
629 | -1.2150574 -0.8463503 15.336658
630 | -2.421735 0.1592144 10.251373
631 | -0.7793127 2.4145525 6.880337
632 | 0.7625532 3.2908301 7.569867
633 | -0.7410054 1.7345991 8.743026
634 | -1.5598224 0.7769185 8.69993
635 | -1.1192893 0.9540894 8.115746
636 | -0.8846575 1.3754689 7.2634096
637 | -0.78410107 1.7010803 6.4062853
638 | -0.6931214 1.887828 6.071097
639 | -0.51595044 1.8543092 7.105392
640 | -0.37708676 1.6867151 8.230667
641 | -0.34356794 1.8495208 8.379107
642 | -0.09935937 2.2661119 8.000824
643 | -0.2621651 2.5438392 8.245032
644 | -0.48722005 3.0753522 8.393473
645 | -0.6691794 3.3626564 8.3408
646 | -0.61171854 3.515885 8.393473
647 | -0.7745243 3.3913867 8.585009
648 | -0.6835446 3.252523 9.0016
649 | -0.30047232 3.453636 9.308058
650 | -0.046686932 3.7840357 9.379884
651 | 0.33638534 4.167108 9.485229
652 | 0.6572084 4.368221 9.973646
653 | 0.46567222 3.9755719 10.912173
654 | 0.16879122 3.3243492 11.889007
655 | 0.24540567 2.9460652 11.917738
656 | -0.089782566 3.525462 11.137228
657 | 1.1695676 3.693056 8.714295
658 | 1.9596541 2.7641058 9.054273
659 | 2.3235726 0.724246 14.455591
660 | 0.59974754 -1.435324 17.94155
661 | -0.6691794 -0.91338795 12.918514
662 | 0.22146365 0.61411273 7.7183075
663 | 1.6053122 1.2509704 7.1245456
664 | 2.778471 1.1456255 9.834782
665 | 2.0602105 0.8583213 10.663176
666 | 1.1312603 1.3036429 9.308058
667 | 0.676362 1.9548657 7.057508
668 | 0.48003745 2.2709002 5.5778913
669 | 0.3842694 2.409764 4.8021703
670 | 0.24540567 2.0506337 5.2810106
671 | 0.30286652 1.6436194 6.9425864
672 | 0.48003745 1.3467385 8.719084
673 | 0.6476316 1.4520833 9.513959
674 | 0.5614403 1.5334862 10.088568
675 | 0.17836803 1.7633295 9.576208
676 | 0.22625206 2.2517467 8.666411
677 | 0.676362 2.835932 8.245032
678 | 0.81522566 3.228581 8.254609
679 | 0.78649527 3.6020765 7.9529395
680 | 0.3890578 3.8127663 7.972093
681 | 0.09696517 3.8175547 8.3551655
682 | 0.044292733 3.582923 9.6767645
683 | 


--------------------------------------------------------------------------------
/src/pedometer/pedometerTest.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/pedometer/pedometerTest.java


--------------------------------------------------------------------------------
/src/simulator/FileWrite.java:
--------------------------------------------------------------------------------
 1 | package simulator;
 2 | 
 3 | import java.io.BufferedWriter;
 4 | import java.io.File;
 5 | import java.io.FileWriter;
 6 | import java.io.IOException;
 7 | 
 8 | public class FileWrite {
 9 | 	private String filePath;
10 | 	private BufferedWriter bw;
11 | 	
12 | 	public FileWrite(String filePath)
13 | 	{
14 | 		this.filePath=filePath;
15 | 		initial();
16 | 	}
17 | 	
18 | 	private void initial()
19 | 	{
20 | 		File file = new File(filePath);
21 | 		if (!file.exists()) {
22 | 			try {
23 | 				file.createNewFile();
24 | 			} catch (IOException e) {
25 | 				// TODO Auto-generated catch block
26 | 				e.printStackTrace();
27 | 			}
28 | 		}
29 | 		FileWriter fw = null;
30 | 		try {
31 | 			fw = new FileWriter(file.getAbsoluteFile());
32 | 		} catch (IOException e) {
33 | 			// TODO Auto-generated catch block
34 | 			e.printStackTrace();
35 | 		}
36 | 		bw = new BufferedWriter(fw);
37 | 	}
38 | 	
39 | 	public void write(String content)
40 | 	{
41 | 		try {
42 | 			bw.write(content);
43 | 		} catch (IOException e) {
44 | 			// TODO Auto-generated catch block
45 | 			e.printStackTrace();
46 | 		}
47 | 	}
48 | 	
49 | 	public void closeStream()
50 | 	{
51 | 		try {
52 | 			bw.close();
53 | 			//System.out.print("file closed");
54 | 		} catch (IOException e) {
55 | 			// TODO Auto-generated catch block
56 | 			e.printStackTrace();
57 | 		}
58 | 	}
59 | }
60 | 


--------------------------------------------------------------------------------
/src/simulator/Simulate.java:
--------------------------------------------------------------------------------
  1 | package simulator;
  2 | 
  3 | import java.awt.Color;
  4 | import java.awt.EventQueue;
  5 | import javax.swing.JFrame;
  6 | import javax.swing.JPanel;
  7 | import javax.swing.border.EmptyBorder;
  8 | import javax.swing.JButton;
  9 | import java.awt.Graphics;
 10 | import java.awt.Graphics2D;
 11 | import java.awt.Image;
 12 | import java.awt.List;
 13 | import java.awt.Point;
 14 | 
 15 | import javax.swing.JTextField;
 16 | import java.awt.event.ActionListener;
 17 | import java.awt.image.BufferedImage;
 18 | import java.util.LinkedList;
 19 | import java.util.Random;
 20 | import java.awt.event.ActionEvent;
 21 | import javax.swing.JLabel;
 22 | import javax.swing.GroupLayout;
 23 | import javax.swing.GroupLayout.Alignment;
 24 | import javax.swing.LayoutStyle.ComponentPlacement;
 25 | import java.awt.event.MouseAdapter;
 26 | import java.awt.event.MouseEvent;
 27 | import javax.swing.JTextArea;
 28 | import javax.swing.JCheckBox;
 29 | import javax.swing.event.ChangeListener;
 30 | import particlefilter.FileWrite;
 31 | import particlefilter.Particle;
 32 | import particlefilter.ParticleFilter;
 33 | import util.Matrix;
 34 | import util.Point2D;
 35 | import util.Point2D;
 36 | 
 37 | import javax.swing.event.ChangeEvent;
 38 | 
 39 | import kalmanfilter.*;
 40 | 
 41 | /**
 42 |  * Encouraged by https://github.com/erhs-53-hackers/Particle-Filter/tree/master/src/particlefilter
 43 |  * 
 44 |  * @author LeslieXong
 45 |  *
 46 |  */
 47 | public class Simulate extends JFrame
 48 | {
 49 | 	private static final long serialVersionUID = 1L;
 50 | 
 51 | 	private ParticleFilter particleFilter;
 52 | 	private Particle insEstimator;
 53 | 	private Random random;
 54 | 
 55 | 	private KalmanFilter kalmanFilter;
 56 | 
 57 | 	private static final Point2D[] landmarks = new Point2D[] { new Point2D(10.5, 16.0f), new Point2D(62.5, 42.0f),
 58 | 			new Point2D(22.5f, 40.0f), new Point2D(39.9f, 25.0f),
 59 | 			new Point2D(63.0, 14.0f), new Point2D(33.7f, 4.2f) };
 60 | 
 61 | 	final int PARTICLES_NUM = 3000;
 62 | 	final float WORLD_WIDTH = 75f, WORLD_HEIGHT = 50f;
 63 | 	final int G_MARGIN_X = 12, G_MARGIN_Y = 35;
 64 | 	final float SCALE = 10f;  //pixel per meter
 65 | 	int drawWidth;
 66 | 	int drawHeight;
 67 | 
 68 | 	private Image image;
 69 | 	private Graphics2D graphics;
 70 | 
 71 | 	private JPanel contentPane;
 72 | 	private JTextField textFieldMoveNoise;
 73 | 	private JTextField textFieldTurnNoise;
 74 | 	private JTextField textFieldSenseNoise;
 75 | 	private JTextArea txtNotice;
 76 | 
 77 | 	//default display
 78 | 	private boolean showKF = true;
 79 | 	private boolean showPF = true;
 80 | 
 81 | 	private boolean showMLE = true;
 82 | 	private boolean showINS = true;
 83 | 	private boolean showParticles = true;
 84 | 
 85 | 	private float moveNoise = 1;
 86 | 	private float orientNoise = 8;
 87 | 	private float senseNoise = 3;
 88 | 
 89 | 	private LinkedList<Point2D> truePosition = new LinkedList<Point2D>();
 90 | 	private LinkedList<Point2D> kfPosition = new LinkedList<Point2D>();
 91 | 	private LinkedList<Point2D> pfPosition = new LinkedList<Point2D>();
 92 | 	private LinkedList<Point2D> mlePosition = new LinkedList<Point2D>();
 93 | 	private LinkedList<Point2D> insPosition = new LinkedList<Point2D>();
 94 | 
 95 | 	private float insError = 0;
 96 | 	private float pfError = 0;
 97 | 	private float kfError = 0;
 98 | 	private float mleError = 0;
 99 | 
100 | 	/**
101 | 	 * Launch the application.
102 | 	 */
103 | 	public static void main(String[] args)
104 | 	{
105 | 		EventQueue.invokeLater(new Runnable()
106 | 		{
107 | 			public void run()
108 | 			{
109 | 				try
110 | 				{
111 | 					Simulate frame = new Simulate();
112 | 					frame.setVisible(true);
113 | 				} catch (Exception e)
114 | 				{
115 | 					e.printStackTrace();
116 | 				}
117 | 			}
118 | 		});
119 | 	}
120 | 
121 | 	private void setUp()
122 | 	{
123 | 		insError = 0;
124 | 		pfError = 0;
125 | 		kfError = 0;
126 | 		mleError = 0;
127 | 		random = new Random();
128 | 		kalmanFilter = null;
129 | 
130 | 		truePosition = new LinkedList<Point2D>();
131 | 		kfPosition = new LinkedList<Point2D>();
132 | 		pfPosition = new LinkedList<Point2D>();
133 | 		mlePosition = new LinkedList<Point2D>();
134 | 		insPosition = new LinkedList<>();
135 | 
136 | 		insEstimator = new Particle(landmarks, WORLD_WIDTH, WORLD_HEIGHT);
137 | 		insEstimator.setSenseNoise(senseNoise);
138 | 
139 | 		particleFilter = new ParticleFilter(PARTICLES_NUM, landmarks, WORLD_WIDTH, WORLD_HEIGHT);
140 | 		particleFilter.setSenseNoise(senseNoise);
141 | 		
142 | 		drawWidth = (int) (WORLD_WIDTH * SCALE);
143 | 		drawHeight = (int) (WORLD_HEIGHT * SCALE);
144 | 		image = new BufferedImage(drawWidth, (int) drawHeight, BufferedImage.BITMASK);
145 | 		graphics = (Graphics2D) image.getGraphics();
146 | 		graphics.drawRect(0, 0, drawWidth - 1, drawHeight - 1);
147 | 	}
148 | 
149 | 	@Override
150 | 	public void update(Graphics g)
151 | 	{
152 | 		paint(g);
153 | 	}
154 | 
155 | 	private void drawPoints(boolean ifdraw, Color color, Object[] points, int r)
156 | 	{
157 | 		if (ifdraw)
158 | 		{
159 | 			graphics.setPaint(color);
160 | 			for (Object o : points)
161 | 			{
162 | 				Point2D p = (Point2D) o;
163 | 				graphics.fillOval((int) (p.x * SCALE) - r / 2, (int) (p.y * SCALE) - r / 2, r, r);
164 | 			}
165 | 		}
166 | 	}
167 | 
168 | 	@Override
169 | 	public void paint(Graphics g)
170 | 	{
171 | 		super.paint(g);
172 | 
173 | 		/* Draw simulation environment ****************************************/
174 | 		graphics.clearRect(0, 0, drawWidth, drawHeight);
175 | 		graphics.setPaint(Color.white);
176 | 		graphics.drawRect(0, 0, drawWidth - 1, drawHeight - 1);
177 | 
178 | 		/* Draw all landmarks *************************************************/
179 | 		drawPoints(true, Color.CYAN, landmarks, 12);
180 | 
181 | 		/* Draw the true trajectory ********************************************/
182 | 		drawPoints(true, Color.RED, truePosition.toArray(), 15);
183 | 
184 | 		/* Draw all particles *************************************************/
185 | 		int size = particleFilter.particles.length;
186 | 		Point2D[] particles = new Point2D[size];
187 | 		for (int i = 0; i < size; i++)
188 | 		{
189 | 			particles[i] = new Point2D(particleFilter.particles[i].x, particleFilter.particles[i].y);
190 | 		}
191 | 		drawPoints(showParticles, Color.PINK, particles, 2);
192 | 
193 | 		drawPoints(showMLE, Color.YELLOW, mlePosition.toArray(), 10);
194 | 		drawPoints(showKF, Color.GREEN, kfPosition.toArray(), 10);
195 | 		drawPoints(showPF, Color.BLUE, pfPosition.toArray(), 10);
196 | 		drawPoints(showINS, Color.magenta, insPosition.toArray(), 10);
197 | 
198 | 		/* Draw current position info ********************************************/
199 | 		if (truePosition.size() > 0)
200 | 		{
201 | 			graphics.setPaint(Color.red);
202 | 			graphics.drawString(String.format("X( Width: %.1fm)  %.1f", WORLD_WIDTH, truePosition.getLast().x), 10, 20);
203 | 			graphics.drawString(String.format("Y( Height: %.1fm) %.1f", WORLD_HEIGHT, truePosition.getLast().y), 10,
204 | 					40);
205 | 			txtNotice.setText(
206 | 					String.format("MLE:%.2fm\nINS:%.2fm \nKF:%.2fm\nPF:%.2fm\n", mleError, insError, kfError, pfError));
207 | 		}
208 | 
209 | 		g.drawImage(image, G_MARGIN_X, G_MARGIN_Y, rootPane);
210 | 	}
211 | 
212 | 	private void saveResult()
213 | 	{
214 | 		txtNotice.setText("Saving...");
215 | 		String file = "src/pfGUI/simulate-out.csv";
216 | 		FileWrite fw = new FileWrite(file);
217 | 		String c = String.format("True,%.2f,MLE,%.2f,KF,%.2f,PF,%.2f,INS,%.2f\n", 0f, mleError, kfError, pfError,
218 | 				insError);
219 | 		fw.write(c);
220 | 		for (int i = 0; i < truePosition.size(); i++)
221 | 		{
222 | 			String s = String.format("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",
223 | 					truePosition.get(i).x, truePosition.get(i).y, mlePosition.get(i).x,
224 | 					mlePosition.get(i).y, kfPosition.get(i).x, kfPosition.get(i).y, pfPosition.get(i).x,
225 | 					pfPosition.get(i).y, insPosition.get(i).x, insPosition.get(i).y);
226 | 			fw.write(s);
227 | 		}
228 | 
229 | 		fw.closeStream();
230 | 		txtNotice.setText("data saved in:\n" + file);
231 | 	}
232 | 
233 | 	private void updateNoise()
234 | 	{
235 | 		moveNoise = Float.parseFloat(textFieldMoveNoise.getText());
236 | 		orientNoise = (float) Math.toRadians(Double.parseDouble(textFieldTurnNoise.getText()));
237 | 		senseNoise = Float.parseFloat(textFieldSenseNoise.getText());
238 | 
239 | 		particleFilter.setSenseNoise(senseNoise);
240 | 		insEstimator.setSenseNoise(senseNoise);
241 | 	}
242 | 
243 | 	/**
244 | 	 * Use distance measurement to estimate the position
245 | 	 * 
246 | 	 * @param Z
247 | 	 * @return
248 | 	 */
249 | 	private Point2D getDisAloneEstimation(float[] Z)
250 | 	{
251 | 		ParticleFilter mleEstimator = new ParticleFilter(PARTICLES_NUM, landmarks, WORLD_WIDTH, WORLD_HEIGHT);
252 | 		mleEstimator.setSenseNoise(senseNoise);
253 | 
254 | 		try
255 | 		{
256 | 			return mleEstimator.getEapPosition(Z,1);
257 | 		} catch (Exception e)
258 | 		{
259 | 			e.printStackTrace();
260 | 		}
261 | 		return null;
262 | 	}
263 | 
264 | 	/**
265 | 	 * Create the frame.
266 | 	 */
267 | 	public Simulate()
268 | 	{
269 | 		setUp();
270 | 		setTitle("Particle/Kalman filter Simulator(By lesliexong@qq.com)");
271 | 		setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
272 | 		setBounds(100, 100, 1000, 550);
273 | 		contentPane = new JPanel();
274 | 
275 | 		//choose a new position
276 | 		contentPane.addMouseListener(new MouseAdapter()
277 | 		{
278 | 			@Override
279 | 			public void mouseClicked(MouseEvent e)
280 | 			{
281 | 				updateNoise();
282 | 
283 | 				float x = e.getX() / SCALE;
284 | 				float y = e.getY() / SCALE;
285 | 
286 | 				try
287 | 				{
288 | 					if (truePosition.size() != 0)
289 | 					{
290 | 						float deltax = x - truePosition.getLast().x;
291 | 						float deltay = y - truePosition.getLast().y;
292 | 
293 | 						//simulate the move sensing observing
294 | 						float foward = (float) Math.sqrt(deltax * deltax + deltay * deltay)
295 | 								+ (float) random.nextGaussian() * moveNoise;  //may <0?
296 | 						float direction = (float) Math.atan2(deltay, deltax)
297 | 								+ (float) random.nextGaussian() * orientNoise + 0.1f; // system error,radians?
298 | 
299 | 						insEstimator.move(direction, foward);
300 | 						insError += (Utils.distance(insEstimator.x, insEstimator.y, x, y) - insError)
301 | 								/ (truePosition.size() + 1);
302 | 						insPosition.add(new Point2D(insEstimator.x, insEstimator.y));
303 | 
304 | 						double[][] pdr = { { foward * Math.cos(direction), 0 }, { 0, foward * Math.cos(direction) } };
305 | 						kalmanFilter.setStateTransitModelF(new Matrix(pdr));
306 | 						kalmanFilter.setProcessNoiseCovQ(moveNoise * 1.3f);  //Set transit Noise include angle error
307 | 
308 | 						particleFilter.move(direction, foward);
309 | 					} else
310 | 					{
311 | 						insEstimator.set(x, y, 0, 0);  //initial
312 | 						insPosition.add(new Point2D(x, y));
313 | 					}
314 | 					truePosition.add(new Point2D(x, y));
315 | 
316 | 					float[] Z = Utils.simulateSense(landmarks, senseNoise, new Point2D(x, y));
317 | 
318 | 					Point2D pos = particleFilter.getEapPosition(Z,1);
319 | 					particleFilter.reSample2();
320 | 					pfPosition.add(new Point2D(pos.x, pos.y));
321 | 					pfError += (Utils.distance(pos.x, pos.y, x, y) - pfError) / truePosition.size();
322 | 
323 | 					pos = getDisAloneEstimation(Z);
324 | 					mlePosition.add(new Point2D(pos.x, pos.y));
325 | 					mleError += (Utils.distance(pos.x, pos.y, x, y) - mleError) / truePosition.size();
326 | 
327 | 					double[] mlePos = { pos.x, pos.y };
328 | 					Matrix zMatrix = new Matrix(mlePos).trans();
329 | 
330 | 					if (kalmanFilter == null)
331 | 					{
332 | 						kalmanFilter = new KalmanFilter(2, 2);
333 | 						kalmanFilter.setCurrentState(zMatrix, new Matrix(2, senseNoise * 0.5));
334 | 					} else
335 | 					{
336 | 						kalmanFilter.setObsvNoiseCovR(senseNoise * 0.5);
337 | 						Matrix state = kalmanFilter.filter(zMatrix);
338 | 						pos = new Point2D(state.value(0, 0), state.value(1, 0));
339 | 						kfPosition.add(pos);
340 | 						kfError += (Utils.distance(pos.x, pos.y, x, y) - kfError) / truePosition.size();
341 | 					}
342 | 				} catch (Exception ex)
343 | 				{
344 | 					System.out.println(ex.getMessage());
345 | 				}
346 | 				Simulate.this.repaint();
347 | 			}
348 | 		});
349 | 
350 | 		contentPane.setBorder(new EmptyBorder(5, 5, 5, 5));
351 | 		setContentPane(contentPane);
352 | 
353 | 		JLabel lblNoiseSet = new JLabel("Noise std set:");
354 | 
355 | 		JLabel lblMoveNiose = new JLabel("Move Noise(meter)");
356 | 		textFieldMoveNoise = new JTextField();
357 | 		textFieldMoveNoise.setToolTipText("Meters");
358 | 		textFieldMoveNoise.setText(String.valueOf(moveNoise));
359 | 		textFieldMoveNoise.setColumns(10);
360 | 
361 | 		JLabel lblOrientNoise = new JLabel("Orient Noise(degree)");
362 | 		textFieldTurnNoise = new JTextField();
363 | 		textFieldTurnNoise.setToolTipText("Degree");
364 | 		textFieldTurnNoise.setText(String.valueOf(orientNoise));
365 | 		textFieldTurnNoise.setColumns(10);
366 | 
367 | 		JLabel lblSenseNoise = new JLabel("Sense Noise(meter)");
368 | 		textFieldSenseNoise = new JTextField();
369 | 		textFieldSenseNoise.setToolTipText("Meters");
370 | 		textFieldSenseNoise.setText(String.valueOf(senseNoise));
371 | 		textFieldSenseNoise.setColumns(10);
372 | 
373 | 		JButton btnSave = new JButton("Save Result");
374 | 		btnSave.addActionListener(new ActionListener()
375 | 		{
376 | 			public void actionPerformed(ActionEvent e)
377 | 			{
378 | 				saveResult();
379 | 			}
380 | 		});
381 | 
382 | 		JButton btnReset = new JButton("Reset");
383 | 		btnReset.addActionListener(new ActionListener()
384 | 		{
385 | 			public void actionPerformed(ActionEvent e)
386 | 			{
387 | 				setUp();
388 | 				Simulate.this.repaint();
389 | 			}
390 | 		});
391 | 
392 | 		JLabel lblShow = new JLabel("Display set:");
393 | 
394 | 		final JCheckBox chckbxKF = new JCheckBox("Kalman Filter");
395 | 		chckbxKF.setBackground(Color.GREEN);
396 | 		chckbxKF.addChangeListener(new ChangeListener()
397 | 		{
398 | 			public void stateChanged(ChangeEvent e)
399 | 			{
400 | 				showKF = chckbxKF.isSelected();
401 | 				Simulate.this.repaint();
402 | 			}
403 | 		});
404 | 		chckbxKF.setSelected(showKF);
405 | 
406 | 		final JCheckBox chckbxPF = new JCheckBox("Particle Filter");
407 | 		chckbxPF.setForeground(Color.WHITE);
408 | 		chckbxPF.setBackground(Color.BLUE);
409 | 		chckbxPF.addChangeListener(new ChangeListener()
410 | 		{
411 | 			public void stateChanged(ChangeEvent e)
412 | 			{
413 | 				showPF = chckbxPF.isSelected();
414 | 				Simulate.this.repaint();
415 | 			}
416 | 		});
417 | 		chckbxPF.setSelected(showPF);
418 | 
419 | 		final JCheckBox chckbxMle = new JCheckBox("MLE Alone(triangle)");
420 | 		chckbxMle.setBackground(Color.YELLOW);
421 | 		chckbxMle.addChangeListener(new ChangeListener()
422 | 		{
423 | 			public void stateChanged(ChangeEvent e)
424 | 			{
425 | 				showMLE = chckbxMle.isSelected();
426 | 				Simulate.this.repaint();
427 | 			}
428 | 		});
429 | 		chckbxMle.setSelected(showMLE);
430 | 
431 | 		final JCheckBox chckbxParticles = new JCheckBox("Particles");
432 | 		chckbxParticles.setBackground(Color.PINK);
433 | 		chckbxParticles.addChangeListener(new ChangeListener()
434 | 		{
435 | 			public void stateChanged(ChangeEvent e)
436 | 			{
437 | 				showParticles = chckbxParticles.isSelected();
438 | 				Simulate.this.repaint();
439 | 			}
440 | 		});
441 | 		chckbxParticles.setSelected(showParticles);
442 | 
443 | 		txtNotice = new JTextArea();
444 | 		txtNotice.setText(
445 | 				"Notice:\n   Click on graphic step by step to \n create a trajectory for simulating \n tracking.");
446 | 
447 | 		final JCheckBox chckbxIns = new JCheckBox("INS Alone");
448 | 		chckbxIns.setBackground(Color.MAGENTA);
449 | 		chckbxIns.addChangeListener(new ChangeListener()
450 | 		{
451 | 			public void stateChanged(ChangeEvent e)
452 | 			{
453 | 				showINS = chckbxIns.isSelected();
454 | 				Simulate.this.repaint();
455 | 			}
456 | 		});
457 | 		chckbxIns.setSelected(showINS);
458 | 
459 | 		GroupLayout gl_contentPane = new GroupLayout(contentPane);
460 | 		gl_contentPane.setHorizontalGroup(
461 | 				gl_contentPane.createParallelGroup(Alignment.TRAILING)
462 | 						.addGroup(gl_contentPane.createSequentialGroup()
463 | 								.addContainerGap(774, Short.MAX_VALUE)
464 | 								.addGroup(gl_contentPane.createParallelGroup(Alignment.LEADING)
465 | 										.addComponent(txtNotice, GroupLayout.PREFERRED_SIZE, 180,
466 | 												GroupLayout.PREFERRED_SIZE)
467 | 										.addComponent(lblShow)
468 | 										.addComponent(chckbxKF)
469 | 										.addComponent(chckbxPF)
470 | 										.addComponent(chckbxIns)
471 | 										.addGroup(gl_contentPane.createParallelGroup(Alignment.TRAILING)
472 | 												.addGroup(gl_contentPane.createSequentialGroup()
473 | 														.addGroup(gl_contentPane
474 | 																.createParallelGroup(Alignment.TRAILING, false)
475 | 																.addComponent(btnReset, Alignment.LEADING,
476 | 																		GroupLayout.DEFAULT_SIZE,
477 | 																		GroupLayout.DEFAULT_SIZE, Short.MAX_VALUE)
478 | 																.addComponent(btnSave, Alignment.LEADING,
479 | 																		GroupLayout.DEFAULT_SIZE, 115, Short.MAX_VALUE))
480 | 														.addGap(75))
481 | 												.addGroup(gl_contentPane.createParallelGroup(Alignment.TRAILING)
482 | 														.addGroup(gl_contentPane.createSequentialGroup()
483 | 																.addGroup(gl_contentPane
484 | 																		.createParallelGroup(Alignment.TRAILING, false)
485 | 																		.addComponent(lblSenseNoise, Alignment.LEADING,
486 | 																				GroupLayout.DEFAULT_SIZE,
487 | 																				GroupLayout.DEFAULT_SIZE,
488 | 																				Short.MAX_VALUE)
489 | 																		.addComponent(lblMoveNiose, Alignment.LEADING,
490 | 																				GroupLayout.DEFAULT_SIZE,
491 | 																				GroupLayout.DEFAULT_SIZE,
492 | 																				Short.MAX_VALUE)
493 | 																		.addComponent(lblOrientNoise,
494 | 																				Alignment.LEADING))
495 | 																.addGap(33)
496 | 																.addGroup(gl_contentPane
497 | 																		.createParallelGroup(Alignment.LEADING, false)
498 | 																		.addComponent(textFieldSenseNoise, 0, 0,
499 | 																				Short.MAX_VALUE)
500 | 																		.addComponent(textFieldTurnNoise,
501 | 																				GroupLayout.PREFERRED_SIZE, 37,
502 | 																				GroupLayout.PREFERRED_SIZE)))
503 | 														.addComponent(textFieldMoveNoise, GroupLayout.PREFERRED_SIZE,
504 | 																37, GroupLayout.PREFERRED_SIZE)
505 | 														.addComponent(lblNoiseSet, Alignment.LEADING)))
506 | 										.addGroup(gl_contentPane.createParallelGroup(Alignment.TRAILING, false)
507 | 												.addComponent(chckbxParticles, Alignment.LEADING, 0, 0, Short.MAX_VALUE)
508 | 												.addComponent(chckbxMle, Alignment.LEADING, GroupLayout.DEFAULT_SIZE,
509 | 														GroupLayout.DEFAULT_SIZE, Short.MAX_VALUE)))
510 | 								.addContainerGap(GroupLayout.DEFAULT_SIZE, Short.MAX_VALUE)));
511 | 		gl_contentPane.setVerticalGroup(
512 | 				gl_contentPane.createParallelGroup(Alignment.LEADING)
513 | 						.addGroup(gl_contentPane.createSequentialGroup()
514 | 								.addComponent(lblNoiseSet)
515 | 								.addPreferredGap(ComponentPlacement.UNRELATED)
516 | 								.addGroup(gl_contentPane.createParallelGroup(Alignment.BASELINE)
517 | 										.addComponent(textFieldMoveNoise, GroupLayout.PREFERRED_SIZE,
518 | 												GroupLayout.DEFAULT_SIZE, GroupLayout.PREFERRED_SIZE)
519 | 										.addComponent(lblMoveNiose))
520 | 								.addPreferredGap(ComponentPlacement.UNRELATED)
521 | 								.addGroup(gl_contentPane.createParallelGroup(Alignment.BASELINE)
522 | 										.addComponent(textFieldTurnNoise, GroupLayout.PREFERRED_SIZE,
523 | 												GroupLayout.DEFAULT_SIZE, GroupLayout.PREFERRED_SIZE)
524 | 										.addComponent(lblOrientNoise))
525 | 								.addPreferredGap(ComponentPlacement.UNRELATED)
526 | 								.addGroup(gl_contentPane.createParallelGroup(Alignment.BASELINE)
527 | 										.addComponent(textFieldSenseNoise, GroupLayout.PREFERRED_SIZE,
528 | 												GroupLayout.DEFAULT_SIZE, GroupLayout.PREFERRED_SIZE)
529 | 										.addComponent(lblSenseNoise))
530 | 								.addGap(33)
531 | 								.addComponent(lblShow)
532 | 								.addPreferredGap(ComponentPlacement.RELATED, 9, Short.MAX_VALUE)
533 | 								.addComponent(chckbxIns)
534 | 								.addPreferredGap(ComponentPlacement.RELATED)
535 | 								.addComponent(chckbxKF)
536 | 								.addGap(1)
537 | 								.addComponent(chckbxPF)
538 | 								.addPreferredGap(ComponentPlacement.RELATED)
539 | 								.addComponent(chckbxParticles)
540 | 								.addGap(2)
541 | 								.addComponent(chckbxMle)
542 | 								.addGap(26)
543 | 								.addComponent(btnSave)
544 | 								.addPreferredGap(ComponentPlacement.UNRELATED)
545 | 								.addComponent(btnReset)
546 | 								.addGap(18)
547 | 								.addComponent(txtNotice, GroupLayout.PREFERRED_SIZE, 76, GroupLayout.PREFERRED_SIZE)
548 | 								.addGap(42)));
549 | 		contentPane.setLayout(gl_contentPane);
550 | 	}
551 | }
552 | 


--------------------------------------------------------------------------------
/src/simulator/Utils.java:
--------------------------------------------------------------------------------
 1 | package simulator;
 2 | 
 3 | import java.util.Random;
 4 | 
 5 | import util.Point2D;
 6 | 
 7 | /**
 8 |  *
 9 |  * @author LeslieXong
10 |  */
11 | public class Utils
12 | {
13 | 	public static double distance(float x1, float y1, float x2, float y2)
14 | 	{
15 | 		return Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
16 | 	}
17 | 
18 | 	public static double gaussianPdf(double mean, double std, double x)
19 | 	{
20 | 		return Math.exp(-(Math.pow(mean - x, 2)) / (2.0 * Math.pow(std, 2)))
21 | 				/ (std*Math.sqrt(2.0 * Math.PI));
22 | 	}
23 | 	
24 | 	
25 | 	  /**
26 |      * Use senseNoise to simulate the distance measurement of the particle to each of its landmarks
27 |      * 
28 |      * @return a float array of distances to landmarks
29 |      */
30 |     public static float[] simulateSense(Point2D[] landmarks,float senseNoise,Point2D currentPosition) {
31 |         float[] ret = new float[landmarks.length];
32 |         Random random =new Random();
33 |         
34 |         for(int i=0;i<landmarks.length;i++){
35 |             float dist = (float) Utils.distance(currentPosition.x, currentPosition.y, landmarks[i].x, landmarks[i].y);
36 |             ret[i] = dist + (float)random.nextGaussian() * senseNoise;
37 |         }       
38 |         return ret;
39 |     }    
40 |     
41 | }
42 | 


--------------------------------------------------------------------------------
/src/util/Matrix.java:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LeslieXong/JAlgorithm/e50a20934f08bee441d62aa4a90291dd591daca3/src/util/Matrix.java


--------------------------------------------------------------------------------
/src/util/Point2D.java:
--------------------------------------------------------------------------------
 1 | package util;
 2 | 
 3 | /**
 4 |  *
 5 |  * @author LeslieXong
 6 |  */
 7 | public class Point2D {
 8 |     
 9 |     public float x, y;
10 | 
11 |     public Point2D() { }
12 | 
13 |     public Point2D(float x, float y) {
14 |         this.x = x;
15 |         this.y = y;
16 |     }
17 |      
18 |     public Point2D(double x,double y)
19 |     {
20 |     	this.x=(float)x;
21 |     	this.y=(float)y;
22 |     }
23 | }


--------------------------------------------------------------------------------
/src/util/Vector.java:
--------------------------------------------------------------------------------
  1 | package util;
  2 | 
  3 | import util.Matrix;
  4 | 
  5 | class Vector
  6 | {
  7 | 	// number of rows
  8 | 	private int nr;
  9 | 	// a double array to store the data
 10 | 	private double[] X;
 11 | 
 12 | 	// constructor: array to a vector
 13 | 	Vector(double[] a)
 14 | 	{
 15 | 		nr = a.length;
 16 | 		X = new double[nr];
 17 | 		for (int i = 0; i < nr; i++)
 18 | 			X[i] = a[i];
 19 | 	}
 20 | 
 21 | 	// copy constructor: vector to a vector
 22 | 	Vector(Vector a)
 23 | 	{
 24 | 		nr = a.GetNrow();
 25 | 		X = new double[nr];
 26 | 		for (int i = 0; i < nr; i++)
 27 | 			X[i] = a.value(i);
 28 | 	}
 29 | 
 30 | 	// overloaded constructor: constant vector
 31 | 	Vector(int Nr, double a)
 32 | 	{
 33 | 		nr = Nr;
 34 | 		X = new double[nr];
 35 | 		for (int i = 0; i < nr; i++)
 36 | 			X[i] = a;
 37 | 	}
 38 | 
 39 | 	// overloaded constructor: matrix as a vector
 40 | 	Vector(Matrix A)
 41 | 	{
 42 | 		if (A.GetNcol() != 1)
 43 | 		{
 44 | 			System.out.println("A is not a vector");
 45 | 			System.exit(1);
 46 | 		}
 47 | 		nr = A.GetNrow();
 48 | 		X = new double[nr];
 49 | 		for (int i = 0; i < nr; i++)
 50 | 			X[i] = A.value(i, 0);
 51 | 	}
 52 | 
 53 | 	// overloaded constructor: extract column of matrix as a vector
 54 | 	Vector(Matrix A, int j)
 55 | 	{
 56 | 		nr = A.GetNrow();
 57 | 		X = new double[nr];
 58 | 		for (int i = 0; i < nr; i++)
 59 | 			X[i] = A.value(i, j);
 60 | 	}
 61 | 
 62 | 	// overloaded constructor: scalar as a vector
 63 | 	Vector(double a)
 64 | 	{
 65 | 		nr = 1;
 66 | 		X = new double[1];
 67 | 		X[0] = a;
 68 | 	}
 69 | 
 70 | 	//
 71 | 	//
 72 | 	// basic matrix operations
 73 | 	//
 74 | 	//
 75 | 
 76 | 	// vector addition
 77 | 	Vector plus(Vector Y)
 78 | 	{
 79 | 		if (nr != Y.GetNrow())
 80 | 		{
 81 | 			System.out.println("Dimensions not compatible");
 82 | 			System.exit(1);
 83 | 		}
 84 | 		Vector XplusY = new Vector(nr, 0);
 85 | 		for (int i = 0; i < nr; i++)
 86 | 			XplusY.setValue(i, X[i] + Y.value(i));
 87 | 
 88 | 		return XplusY;
 89 | 	}
 90 | 
 91 | 	// vector subtraction
 92 | 	Vector minus(Vector Y)
 93 | 	{
 94 | 		if (nr != Y.GetNrow())
 95 | 		{
 96 | 			System.out.println("Dimensions not compatible");
 97 | 			System.exit(1);
 98 | 		}
 99 | 		Vector XminusY = new Vector(nr, 0);
100 | 		for (int i = 0; i < nr; i++)
101 | 			XminusY.setValue(i, X[i] - Y.value(i));
102 | 
103 | 		return XminusY;
104 | 	}
105 | 
106 | 	// vector multiplication
107 | 
108 | 	// inner product
109 | 	double inner(Vector Y)
110 | 	{
111 | 		if (nr != Y.GetNrow())
112 | 		{
113 | 			System.out.println("Dimensions not compatible");
114 | 			System.exit(1);
115 | 		}
116 | 		double prod = 0;
117 | 		for (int i = 0; i < nr; i++)
118 | 			prod += X[i] * Y.value(i);
119 | 
120 | 		return prod;
121 | 	}
122 | 
123 | 	// outer product
124 | 	Matrix outer(Vector Y)
125 | 	{
126 | 		double temp;
127 | 		Matrix XtimesY = new Matrix(nr, Y.GetNrow(), 0.);
128 | 		for (int i = 0; i < nr; i++)
129 | 			for (int j = 0; j < Y.GetNrow(); j++)
130 | 				XtimesY.setValue(i, j, X[i] * Y.value(j));
131 | 
132 | 		return XtimesY;
133 | 	}
134 | 
135 | 	// scalar multiplication
136 | 	Vector times(double Y)
137 | 	{
138 | 		Vector XtimesY = new Vector(nr, 0);
139 | 		for (int i = 0; i < nr; i++)
140 | 			XtimesY.setValue(i, X[i] * Y);
141 | 
142 | 		return XtimesY;
143 | 	}
144 | 
145 | 	//
146 | 	// access to a member elements
147 | 	//
148 | 
149 | 	double value(int r)
150 | 	{
151 | 		return X[r];
152 | 	}
153 | 
154 | 	int GetNrow()
155 | 	{
156 | 		return nr;
157 | 	}
158 | 
159 | 	void setValue(int r, double a)
160 | 	{
161 | 		X[r] = a;
162 | 	}
163 | 
164 | 	//
165 | 	// utilities
166 | 	//
167 | 	double[] getRetValues()
168 | 	{
169 | 
170 | 		return X;
171 | 
172 | 	}
173 | 
174 | 	void printVector()
175 | 	{
176 | 		//System.out.println(" ");
177 | 		for (int i = 0; i < nr; i++)
178 | 		{
179 | 			System.out.print(X[i] + " ");
180 | 			System.out.println(" ");
181 | 		}
182 | 	}
183 | 
184 | }
185 | 


--------------------------------------------------------------------------------