To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 6c5f1471 authored by beuchatp's avatar beuchatp
Browse files

LQG 1e-3*Qu , 100Hz , printing estimates for copter 1

parent 49a92689
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0.23 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 -0.23 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 -0.19919 0 0 0
0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 -0.115 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0.19919 0.115 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 1 0 0 0 0 0 0 0 0.19919 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 1 0 0 0 0 0 0 -0.115 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 -0.19919 0.115 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
\ No newline at end of file
0.0033603 0 0 0 -0.0029852 0 7.2824e-05 -6.1031e-05 0 -0.00018325 -0.00025103 0 7.2824e-05 6.1031e-05 0 0.00018325 -0.00025103 0 0 -0.0001589 0 0 0 0.0020285
0 0.0034175 0 0.0035578 0 0 0 4.1503e-05 0 -8.707e-05 0 0 0 4.1503e-05 0 -8.707e-05 0 0 0.00055668 0 0 0 0 0
0 0 0.0049875 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0.0060975 0 0.020997 0 0 0 -0.0013204 0 -0.007096 0 0 0 -0.0013204 0 -0.007096 0 0 -0.0034724 0 0 0 0 0
0.0067139 0 0 0 -0.014383 0 -0.0016095 0.00032488 0 -0.00045419 0.0028861 0 -0.0016095 -0.00032488 0 0.00045419 0.0028861 0 0 0.009768 0 0 0 0.031188
0 0.019237 0 0.026737 0 0 0 -0.0066476 0 -0.014799 0 0 0 -0.0066476 0 -0.014799 0 0 -0.0015937 0 0 0 0 0
0.025979 0 0 0 -0.028076 0 -0.009919 0.0053558 0 0.015495 0.013067 0 -0.009919 -0.0053558 0 -0.015495 0.013067 0 0 0.019316 0 0 0 0.086501
7.2824e-05 0 0 0 -0.00018155 0 0.0034009 2.9943e-05 0 8.4711e-05 -0.003397 0 2.4978e-05 -2.9943e-05 0 -8.4711e-05 0.00024783 0 0 -0.00075014 0 0 0 -0.00092067
-6.1031e-05 4.1503e-05 0 -8.707e-05 0.00034313 0 2.9943e-05 0.003378 0 0.0034187 -0.00022043 0 2.9943e-05 8.097e-05 0 5.1967e-05 -0.00022043 0 0.00055668 0.00027902 0 0 0 0.00092837
0 0 0 0 0 0 0 0 0.0049875 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0.00048404 -0.0013204 0 -0.007096 0.0068741 0 -0.00021811 0.0062204 0 0.020568 -0.0040548 0 -0.00021811 -0.0014432 0 -0.0066664 -0.0040548 0 -0.0034724 0.0055387 0 0 0 0.031445
-0.00027787 0 0 0 0.0029809 0 0.0055825 0.00081529 0 0.0047076 -0.019161 0 -0.0018353 -0.00081529 0 -0.0047076 0.0089322 0 0 0.0048399 0 0 0 0.0067421
0.0088074 -0.0066476 0 -0.014799 0.000732 0 -0.0043109 0.025712 0 0.052377 -0.0037039 0 -0.0043109 -0.013122 0 -0.04044 -0.0037039 0 -0.0015937 0.027749 0 0 0 0.14984
0.00098028 0 0 0 0.0050249 0 0.015492 0.006128 0 0.018201 -0.023084 0 -0.010392 -0.006128 0 -0.018201 0.018451 0 0 0.012058 0 0 0 0.055494
7.2824e-05 0 0 0 -0.00018155 0 2.4978e-05 2.9943e-05 0 8.4711e-05 0.00024783 0 0.0034009 -2.9943e-05 0 -8.4711e-05 -0.003397 0 0 -0.00075014 0 0 0 -0.00092067
6.1031e-05 4.1503e-05 0 -8.707e-05 -0.00034313 0 -2.9943e-05 8.097e-05 0 5.1967e-05 0.00022043 0 -2.9943e-05 0.003378 0 0.0034187 0.00022043 0 0.00055668 -0.00027902 0 0 0 -0.00092837
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.0049875 0 0 0 0 0 0 0 0 0
-0.00048404 -0.0013204 0 -0.007096 -0.0068741 0 0.00021811 -0.0014432 0 -0.0066664 0.0040548 0 0.00021811 0.0062204 0 0.020568 0.0040548 0 -0.0034724 -0.0055387 0 0 0 -0.031445
-0.00027787 0 0 0 0.0029809 0 -0.0018353 0.00081529 0 0.0047076 0.0089322 0 0.0055825 -0.00081529 0 -0.0047076 -0.019161 0 0 0.0048399 0 0 0 0.0067421
-0.0088074 -0.0066476 0 -0.014799 -0.000732 0 0.0043109 -0.013122 0 -0.04044 0.0037039 0 0.0043109 0.025712 0 0.052377 0.0037039 0 -0.0015937 -0.027749 0 0 0 -0.14984
0.00098028 0 0 0 0.0050249 0 -0.010392 0.006128 0 0.018201 0.018451 0 0.015492 -0.006128 0 -0.018201 -0.023084 0 0 0.012058 0 0 0 0.055494
0 0.00057617 0 0.0041525 0 0 0 0.00057617 0 0.0041525 0 0 0 0.00057617 0 0.0041525 0 0 0.005872 0 0 0 0 0
-0.0001604 0 0 0 0.0039753 0 -0.00077843 0.00029312 0 0.0013647 0.004047 0 -0.00077843 -0.00029312 0 -0.0013647 0.004047 0 0 0.0072535 0 0 0 0.0071166
0 0 0 0 0 0.0094189 0 0 0 0 0 0.0094189 0 0 0 0 0 0.0094189 0 0 0.0094189 0 0 0
0 0.0039539 0 0.010926 0 0 0 0.0039539 0 0.010926 0 0 0 0.0039539 0 0.010926 0 0 0.0083466 0 0 0 0 0
-0.00025784 0 0 0 0.0079082 0 -0.0057616 0.0028991 0 0.0090673 0.011581 0 -0.0057616 -0.0028991 0 -0.0090673 0.011581 0 0 0.015401 0 0 0 0.037355
0 0 0 0 0 0.03483 0 0 0 0 0 0.03483 0 0 0 0 0 0.03483 0 0 0.03483 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0.013673 0 0 0 0 0 -0.013673 0 0 0 0.068644 0 0
0 0 0 0 0 -0.015788 0 0 0 0 0 0.0078941 0 0 0 0 0 0.0078941 0 0 0 0 0.068644 0
0.0021448 0 0 0 0.0050396 0 -0.00097694 0.0010083 0 0.0039884 -0.0030764 0 -0.00097694 -0.0010083 0 -0.0039884 -0.0030764 0 0 0.0072353 0 0 0 0.060497
0 0 0 0 0 0 0 0 0 0 0 0.097686 0 0 0 0 0 -0.097686 0 0 0 0.49042 0 0
0 0 0 0 0 -0.1128 0 0 0 0 0 0.056399 0 0 0 0 0 0.056399 0 0 0 0 0.49042 0
0.024104 0 0 0 -0.00059422 0 -0.011669 0.016641 0 0.064621 -0.0061471 0 -0.011669 -0.016641 0 -0.064621 -0.0061471 0 0 0.061975 0 0 0 0.38059
\ No newline at end of file
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -32.841 0 0 -1.7185 0 21.7624 0 0 0.63666 0
-18.3241 0 0 0 -29.519 0 -5.5646 1.3442 -3.5525 0 -22.6565 11.9903 -2.9061 1.2739 1.3442 3.5525 0 22.6565 11.9903 2.9061 1.2739 0 10.1503 0 0 5.9742 0 0 0 -6.6654 0 0 -4.1585
0 -14.3434 0 -3.8015 0 -2.2932 0 0 -0.59734 0 -1.2331 0 -0.36014 0 0 -0.59734 0 -1.2331 0 -0.36014 0 -11.1002 0 0 -6.3466 0 0 0 0 0 0 0 0
0 0 -9.7531 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -32.841 0 0 -1.7185 -18.8468 -10.8812 0 -0.55136 -0.31833 0
1.3319 0 0 0 9.8435 0 1.1615 -15.2269 1.681 0 9.7301 -9.4222 1.3301 -3.0178 -1.4809 -1.681 0 -9.7301 -6.8538 -1.3301 -1.0848 0 11.5271 0 0 6.4562 0 0 0 2.6302 0 0 1.489
-3.5532 -0.59734 0 -1.2331 -22.7095 -0.36014 -2.9122 1.6914 -17.4681 0 -23.7208 11.5439 -4.8522 1.4245 1.6914 2.5273 0 18.6862 11.5439 2.1989 1.4245 -11.1002 -0.68844 0 -6.3466 -0.21767 0 0 0 -4.6638 0 0 -2.7951
0 0 0 0 0 0 0 0 0 -9.7531 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -32.841 0 0 -1.7185 18.8468 -10.8812 0 0.55136 -0.31833 0
1.3319 0 0 0 9.8435 0 1.1615 -1.4809 1.681 0 9.7301 -6.8538 1.3301 -1.0848 -15.2269 -1.681 0 -9.7301 -9.4222 -1.3301 -3.0178 0 11.5271 0 0 6.4562 0 0 0 2.6302 0 0 1.489
3.5532 -0.59734 0 -1.2331 22.7095 -0.36014 2.9122 -1.6914 2.5273 0 18.6862 -11.5439 2.1989 -1.4245 -1.6914 -17.4681 0 -23.7208 -11.5439 -4.8522 -1.4245 -11.1002 0.68844 0 -6.3466 0.21767 0 0 0 4.6638 0 0 2.7951
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -9.7531 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
\ No newline at end of file
0.90502 0 0 0 -0.14909 0 -0.027823 0.0066482 -0.017701 0 -0.11319 0.059826 -0.01453 0.0063694 0.0066482 0.017701 0 0.11319 0.059826 0.01453 0.0063694 0 0.054398 0 0 0.029871 0 0 0 -0.0348 0 0 -0.020792
0 0.92487 0 -0.020787 0 -0.011466 0 0 -0.0030282 0 -0.0061219 0 -0.0018007 0 0 -0.0030282 0 -0.0061219 0 -0.0018007 0 -0.059441 0 0 -0.031733 0 0 0 0 0 0 0 0
0 0 0.94625 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 -0.0060975 0 0.9895 0 0.005 0 0 0.0013204 0 0.003548 0 0 0 0 0.0013204 0 0.003548 0 0 0 -0.003333 0 0 0 0 0 0 0 0 0 0 0
-0.0067139 0 0 0 0.99281 0 0.005 0.0016095 -0.00032488 0 0.0002271 0.0014431 0 0 0.0016095 0.00032488 0 -0.0002271 0.0014431 0 0 0 -0.0011577 0 0 0 0 0 0 -0.027397 0 0 0
0 0.10165 0 -0.068859 0 1 0 0 0.0066476 0 -0.025301 0 0 0 0 0.0066476 0 -0.025301 0 0 0 0.004455 0 0 0 0 0 0 0 0 0 0 0
0.094912 0 0 0 -0.033058 0 1 0.009919 -0.0053558 0 0.023837 -0.044402 0 0 0.009919 0.0053558 0 -0.023837 -0.044402 0 0 0 -0.017373 0 0 0 0 0 0 -0.070866 0 0 0
0.0065866 0 0 0 0.049127 0 0.0058077 0.92046 0.0083753 0 0.048608 -0.04881 0.0066506 -0.015089 -0.0074293 -0.0083753 0 -0.048608 -0.034145 -0.0066506 -0.005424 0 0.061716 0 0 0.032281 0 0 0 0.013785 0 0 0.0074448
-0.017705 -0.0030282 0 -0.0061219 -0.11338 -0.0018007 -0.014561 0.008427 0.90928 0 -0.12031 0.057609 -0.024261 0.0071226 0.008427 0.012556 0 0.093405 0.057609 0.010995 0.0071226 -0.059441 -0.0036235 0 -0.031733 -0.0010883 0 0 0 -0.023707 0 0 -0.013975
0 0 0 0 0 0 0 0 0 0.94625 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
-0.00048404 0.0013204 0 0.003548 0.0034371 0 0 0.00021811 -0.0062204 0 0.98972 -0.0020274 0.005 0 0.00021811 0.0014432 0 0.0033332 -0.0020274 0 0 -0.003333 -0.0043032 0 0 0 0 0 0 -0.028534 0 0 0
0.00027787 0 0 0 0.0014904 0 0 -0.0055825 -0.00081529 0 -0.0023538 0.99042 0 0.005 0.0018353 0.00081529 0 0.0023538 0.0044661 0 0 0 0.0024081 0 0 0 0 0 0 -0.0067287 0 0 0
-0.0088074 0.0066476 0 -0.025301 0.044393 0 0 0.0043109 0.095179 0 -0.043551 -0.023865 1 0 0.0043109 0.013122 0 -0.050609 -0.023865 0 0 0.004455 -0.021073 0 0 0 0 0 0 -0.13237 0 0 0
-0.00098028 0 0 0 0.0062832 0 0 0.1054 -0.006128 0 0.022484 -0.085268 0 1 0.010392 0.006128 0 -0.022484 -0.04171 0 0 0 -0.012451 0 0 0 0 0 0 -0.049932 0 0 0
0.0065866 0 0 0 0.049127 0 0.0058077 -0.0074293 0.0083753 0 0.048608 -0.034145 0.0066506 -0.005424 0.92046 -0.0083753 0 -0.048608 -0.04881 -0.0066506 -0.015089 0 0.061716 0 0 0.032281 0 0 0 0.013785 0 0 0.0074448
0.017705 -0.0030282 0 -0.0061219 0.11338 -0.0018007 0.014561 -0.008427 0.012556 0 0.093405 -0.057609 0.010995 -0.0071226 -0.008427 0.90928 0 -0.12031 -0.057609 -0.024261 -0.0071226 -0.059441 0.0036235 0 -0.031733 0.0010883 0 0 0 0.023707 0 0 0.013975
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.94625 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0.00048404 0.0013204 0 0.003548 -0.0034371 0 0 -0.00021811 0.0014432 0 0.0033332 0.0020274 0 0 -0.00021811 -0.0062204 0 0.98972 0.0020274 0.005 0 -0.003333 0.0043032 0 0 0 0 0 0 0.028534 0 0 0
0.00027787 0 0 0 0.0014904 0 0 0.0018353 -0.00081529 0 -0.0023538 0.0044661 0 0 -0.0055825 0.00081529 0 0.0023538 0.99042 0 0.005 0 0.0024081 0 0 0 0 0 0 -0.0067287 0 0 0
0.0088074 0.0066476 0 -0.025301 -0.044393 0 0 -0.0043109 0.013122 0 -0.050609 0.023865 0 0 -0.0043109 0.095179 0 -0.043551 0.023865 1 0 0.004455 0.021073 0 0 0 0 0 0 0.13237 0 0 0
-0.00098028 0 0 0 0.0062832 0 0 0.010392 -0.006128 0 0.022484 -0.04171 0 0 0.1054 0.006128 0 -0.022484 -0.085268 0 1 0 -0.012451 0 0 0 0 0 0 -0.049932 0 0 0
0 -0.00057617 0 -0.0020762 0 0 0 0 -0.00057617 0 -0.0020762 0 0 0 0 -0.00057617 0 -0.0020762 0 0 0 0.98167 0 0 0.005 0 0 0 0 0 0 0 0
0.0001604 0 0 0 0.0019877 0 0 0.00077843 -0.00029312 0 -0.00068235 0.0020235 0 0 0.00077843 0.00029312 0 0.00068235 0.0020235 0 0 0 0.98068 0 0 0.005 0 0 0 -0.0065564 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.96232 0 0 0.005 0 0 0 0 0 0
0 -0.0039539 0 0.010887 0 0 0 0 -0.0039539 0 0.010887 0 0 0 0 -0.0039539 0 0.010887 0 0 0 -0.041125 0 0 1 0 0 0 0 0 0 0 0
0.00025784 0 0 0 0.0058395 0 0 0.0057616 -0.0028991 0 0.011259 -0.019677 0 0 0.0057616 0.0028991 0 -0.011259 -0.019677 0 0 0 -0.046472 0 0 1 0 0 0 -0.032898 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -4.1771 0 0 0.78871 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.92591 0 0 0.005 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.92591 0 0 0.005 0
-0.0021448 0 0 0 0.0025198 0 0 0.00097694 -0.0010083 0 -0.0019942 -0.0015382 0 0 0.00097694 0.0010083 0 0.0019942 -0.0015382 0 0 0 -0.0061221 0 0 0 0 0 0 0.93922 0 0 0.005
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -14.6223 0 0 0.58771 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -14.6223 0 0 0.58771 0
-0.024104 0 0 0 0.11022 0 0 0.011669 -0.016641 0 0.0634 -0.058332 0 0 0.011669 0.016641 0 -0.0634 -0.058332 0 0 0 -0.049087 0 0 0 0 0 0 -0.35612 0 0 1
\ No newline at end of file
......@@ -398,20 +398,20 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// update measurement vector using vicon Data
//get_y_from_CFData( request );
get_y_from_CFData_withLoad( request );
/*
y_only_agent_1 = y_to_controller;
for(int i = 6 ; i < 24 ; ++i )
{
y_only_agent_1(i,0) = 0;
}
*/
}
// compute centralized controller commands
centralized_control_command( request, response );
//display_control_agent1( request, response);
//centralized_control_command( request, response );
display_control_agent1( request, response);
//float delta_z_load_current = request.otherCrazyflies[3].z - load_setpoint[2];
// gravity_force_copterloadsystem -= 0.001*delta_z_load_current;
......@@ -517,15 +517,30 @@ void display_control_agent1( Controller::Request &request, Controller::Response
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired yaw-rate computed = " << outYaw );
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: roll estimate = " << estimator_state_prev_only_agent1[0] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: pitch estimate = " << estimator_state_prev_only_agent1[1] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: yaw estimate = " << estimator_state_prev_only_agent1[2] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi estimate = " << estimator_state_prev_only_agent1[3] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable theta estimate = " << estimator_state_prev_only_agent1[4] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi_dot estimate = " << estimator_state_prev_only_agent1[5] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable ttheta_dot estimate = " << estimator_state_prev_only_agent1[6] );
}
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired yaw-rate computed = " << outYaw );
}
// Debugging
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_only_agent_1(0,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_only_agent_1(1,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_only_agent_1(2,0) );
......@@ -1669,7 +1684,7 @@ int main(int argc, char* argv[]) {
// get current working directory for centralized controller
std::string folderpath_c = getcwd_string();
folderpath_c = folderpath_c.substr(0, folderpath_c.size()-5);
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/centralized_gains/load_measurements/");
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/centralized_gains/load_measurements_qu1e-3/");
// READ MATRICES FROM DATA FOLDER
std::string filename_K = folderpath;
......@@ -1690,7 +1705,7 @@ int main(int argc, char* argv[]) {
centralized_controller.Phi = readMatrix( filename_Phi.append("Phi.txt" ) );
outputMatrix_centralized = readMatrix( filename_C.append( "C.txt" ) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] outputMatrix = " << outputMatrix);
ROS_INFO_STREAM("[STUDENT CONTROLLER] K centralized = " << centralized_controller.K);
// Initialize some state vector of the output feedback controller
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment