├── .gitignore ├── LICENSE ├── MANIFEST.in ├── README.md ├── controlpy ├── __init__.py ├── analysis.py └── synthesis.py ├── examples ├── example_controllability.py ├── example_lqr.py └── example_systemGain.py ├── setup.cfg ├── setup.py └── tests ├── test_analysis.py └── test_synth.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled python modules. 2 | *.pyc 3 | 4 | # Setuptools distribution folder. 5 | /dist/ 6 | 7 | # Python egg metadata, regenerated from source files by setuptools. 8 | /*.egg-info 9 | /.eggs 10 | 11 | # vim temp files: 12 | *.py~ 13 | 14 | #Eclipse/PyDev 15 | .project 16 | .pydevproject 17 | 18 | #Vim Temp files 19 | *~ 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | {one line to give the program's name and a brief idea of what it does.} 635 | Copyright (C) {year} {name of author} 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | {project} Copyright (C) {year} {fullname} 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | 676 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include README.md 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Controlpy 2 | ========= 3 | 4 | A library for commonly used controls algorithms (e.g. creating LQR controllers). An alternative to Richard Murray's "control" package -- however, here we do not require Slycot. 5 | 6 | Current capabilities: 7 | 8 | 1. System analysis: 9 | 1. Test whether a system is stable, controllable, stabilisable, observable, or stabilisable. 10 | 2. Get the uncontrollable/unobservable modes 11 | 3. Compute a system's controllability Gramian (finite horizon, and infinite horizon) 12 | 4. Compute a system's H2 and Hinfinity norm 13 | 2. Synthesis 14 | 1. Create continuous and discrete time LQR controllers 15 | 2. Full-information H2 optimal controller 16 | 3. H2 optimal observer 17 | 4. Full-information Hinf controller 18 | 19 | 20 | How to install 21 | -------------- 22 | Install using pypi, or direct from the Github repository: 23 | 24 | 1. Clone this repository somewhere convenient: `git clone https://github.com/markwmuller/controlpy.git` 25 | 2. Install the package (we'll do a "develop" install, so any changes are immediately available): `pip install -e .` (you'll probably need to be administrator) 26 | 3. You're ready to go: try running the examples in the `example` folder. 27 | 28 | 29 | Testing 30 | ------- 31 | If you want to run the unit tests, you'll need to install cvxpy. 32 | 33 | 34 | Licensing 35 | --------- 36 | `(c) Mark W. Mueller 2016` 37 | 38 | This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. 39 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 40 | 41 | You should have received a copy of the GNU General Public License along with this program. If not, see . 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /controlpy/__init__.py: -------------------------------------------------------------------------------- 1 | #(c) 2014 Mark W. Mueller 2 | __all__ = [] 3 | 4 | from . import analysis 5 | from . import synthesis 6 | 7 | __all__.extend(['analysis', 'synthesis']) 8 | -------------------------------------------------------------------------------- /controlpy/analysis.py: -------------------------------------------------------------------------------- 1 | """ Tools for analysing LTI systems. 2 | 3 | (c) 2014 Mark W. Mueller 4 | """ 5 | 6 | import numpy as np 7 | import scipy.linalg 8 | import scipy.integrate 9 | 10 | 11 | def is_hurwitz(A, tolerance = 1e-9): 12 | '''Test whether the matrix A is Hurwitz (i.e. asymptotically stable). 13 | 14 | tolerance defines the minimum distance we should be from the imaginary axis 15 | to be considered stable. 16 | 17 | ''' 18 | return max(np.real(np.linalg.eig(A)[0])) < -np.abs(tolerance) 19 | 20 | 21 | def uncontrollable_modes(A, B, returnEigenValues = False, tolerance=1e-9): 22 | '''Returns all the uncontrollable modes of the pair A,B. 23 | 24 | tolerance defines the minimum distance we should be from the imaginary axis 25 | to be considered stable. 26 | 27 | Does the PBH test for controllability for the system: 28 | dx = A*x + B*u 29 | 30 | Returns a list of the uncontrollable modes, and (optionally) 31 | the corresponding eigenvalues. 32 | 33 | See Callier & Desoer "Linear System Theory", P. 253 34 | 35 | NOTE!: This can't work if we have repeated eigen-values! TODO FIXME! 36 | ''' 37 | 38 | assert A.shape[0]==A.shape[1], "Matrix A is not square" 39 | assert A.shape[0]==B.shape[0], "Matrices A and B do not align" 40 | 41 | nStates = A.shape[0] 42 | nInputs = B.shape[1] 43 | 44 | eVal, eVec = np.linalg.eig(np.matrix(A)) # todo, matrix cast is ugly. 45 | 46 | uncontrollableModes = [] 47 | uncontrollableEigenValues = [] 48 | 49 | for e,v in zip(eVal, eVec.T): 50 | M = np.matrix(np.zeros([nStates,(nStates+nInputs)]), dtype=complex) 51 | M[:,:nStates] = e*np.identity(nStates) - A 52 | M[:,nStates:] = B 53 | 54 | s = np.linalg.svd(M, compute_uv=False) 55 | if min(s) <= tolerance: 56 | uncontrollableModes.append(v.T[:,0]) 57 | uncontrollableEigenValues.append(e) 58 | 59 | if returnEigenValues: 60 | return uncontrollableModes, uncontrollableEigenValues 61 | else: 62 | return uncontrollableModes 63 | 64 | 65 | 66 | def is_controllable(A, B, tolerance=1e-9): 67 | '''Compute whether the pair (A,B) is controllable. 68 | tolerance defines the minimum distance we should be from the imaginary axis 69 | to be considered stable. 70 | 71 | Returns True if controllable, False otherwise. 72 | ''' 73 | 74 | if uncontrollable_modes(A, B, tolerance=tolerance): 75 | return False 76 | else: 77 | return True 78 | 79 | 80 | 81 | def is_stabilizable(A, B): 82 | '''Compute whether the pair (A,B) is stabilisable. 83 | 84 | Returns True if stabilisable, False otherwise. 85 | ''' 86 | 87 | return is_stabilisable(A, B) 88 | 89 | 90 | def is_stabilisable(A, B): 91 | '''Compute whether the pair (A,B) is stabilisable. 92 | 93 | Returns True if stabilisable, False otherwise. 94 | ''' 95 | 96 | modes, eigVals = uncontrollable_modes(A, B, returnEigenValues=True) 97 | if not modes: 98 | return True #controllable => stabilisable 99 | 100 | if max(np.real(eigVals)) >= 0: 101 | return False 102 | else: 103 | return True 104 | 105 | 106 | def controllability_gramian(A, B, T = np.inf): 107 | '''Compute the causal controllability Gramian of the continuous time system. 108 | 109 | The system is described as 110 | dx = A*x + B*u 111 | 112 | T is the horizon over which to compute the Gramian. If not specified, the 113 | infinite horizon Gramian is computed. Note that the infinite horizon Gramian 114 | only exists for asymptotically stable systems. 115 | 116 | If T is specified, we compute the Gramian as 117 | Wc = integrate exp(A*t)*B*B.H*exp(A.H*t) dt 118 | 119 | Returns the matrix Wc. 120 | ''' 121 | 122 | assert A.shape[0]==A.shape[1], "Matrix A is not square" 123 | assert A.shape[0]==B.shape[0], "Matrix A and B do not align" 124 | 125 | if not np.isfinite(T): 126 | #Infinite time Gramian: 127 | assert is_hurwitz(A), "Can only compute infinite horizon Gramian for a stable system." 128 | 129 | Wc = scipy.linalg.solve_lyapunov(A, -B.dot(B.T)) 130 | return Wc 131 | 132 | # We need to solve the finite time Gramian 133 | # Boils down to solving an ODE: 134 | A = np.array(A,dtype=float) 135 | B = np.array(B,dtype=float) 136 | T = np.float(T) 137 | 138 | def gramian_ode(y, t0, A, B): 139 | temp = np.dot(scipy.linalg.expm(A*t0),B) 140 | dQ = np.dot(temp,np.conj(temp.T)) 141 | 142 | return dQ.reshape((A.shape[0]**2,1))[:,0] 143 | 144 | y0 = np.zeros([A.shape[0]**2,1])[:,0] 145 | out = scipy.integrate.odeint(gramian_ode, y0, [0,T], args=(A,B)) 146 | Q = out[1,:].reshape([A.shape[0], A.shape[0]]) 147 | return Q 148 | 149 | 150 | def unobservable_modes(C, A, returnEigenValues = False): 151 | '''Returns all the unobservable modes of the pair A,C. 152 | 153 | Does the PBH test for observability for the system: 154 | dx = A*x 155 | y = C*x 156 | 157 | Returns a list of the unobservable modes, and (optionally) 158 | the corresponding eigenvalues. 159 | 160 | See Callier & Desoer "Linear System Theory", P. 253 161 | ''' 162 | 163 | return uncontrollable_modes(A.conj().T, C.conj().T, returnEigenValues) 164 | 165 | 166 | def is_observable(C, A): 167 | '''Compute whether the pair (C,A) is observable. 168 | 169 | Returns True if observable, False otherwise. 170 | ''' 171 | 172 | return is_controllable(A.conj().T, C.conj().T) 173 | 174 | 175 | def is_detectable(C, A): 176 | '''Compute whether the pair (C,A) is detectable. 177 | 178 | Returns True if detectable, False otherwise. 179 | ''' 180 | 181 | return is_stabilisable(A.conj().T, C.conj().T) 182 | 183 | 184 | #TODO 185 | # def observability_gramian(A, B, T = np.inf): 186 | # '''Compute the observability Gramian of the continuous time system. 187 | # 188 | # The system is described as 189 | # dx = A*x + B*u 190 | # 191 | # T is the horizon over which to compute the Gramian. If not specified, the 192 | # infinite horizon Gramian is computed. Note that the infinite horizon Gramian 193 | # only exists for asymptotically stable systems. 194 | # 195 | # If T is specified, we compute the Gramian as 196 | # Wc = integrate exp(A*t)*B*B.H*exp(A.H*t) dt 197 | # 198 | # Returns the matrix Wc. 199 | # ''' 200 | # 201 | # assert A.shape[0]==A.shape[1], "Matrix A is not square" 202 | # assert A.shape[0]==B.shape[0], "Matrix A and B do not align" 203 | # 204 | # if not np.isfinite(T): 205 | # #Infinite time Gramian: 206 | # eigVals, eigVecs = scipy.linalg.eig(A) 207 | # assert np.max(np.real(eigVals)) < 0, "Can only compute infinite horizon Gramian for a stable system." 208 | # 209 | # Wc = scipy.linalg.solve_lyapunov(A, -B*B.T) 210 | # return Wc 211 | # 212 | # # We need to solve the finite time Gramian 213 | # # Boils down to solving an ODE: 214 | # A = np.array(A,dtype=float) 215 | # B = np.array(B,dtype=float) 216 | # T = np.float(T) 217 | # 218 | # def gramian_ode(y, t0, A, B): 219 | # temp = np.dot(scipy.linalg.expm(A*t0),B) 220 | # dQ = np.dot(temp,np.conj(temp.T)) 221 | # 222 | # return dQ.reshape((A.shape[0]**2,1))[:,0] 223 | # 224 | # y0 = np.zeros([A.shape[0]**2,1])[:,0] 225 | # out = scipy.integrate.odeint(gramian_ode, y0, [0,T], args=(A,B)) 226 | # Q = out[1,:].reshape([A.shape[0], A.shape[0]]) 227 | # return Q 228 | 229 | 230 | def system_norm_H2(Acl, Bdisturbance, C): 231 | '''Compute a system's H2 norm. 232 | 233 | Acl, Bdisturbance are system matrices, describing the systems dynamics: 234 | dx/dt = Acl*x + Bdisturbance*v 235 | where x is the system state and v is the disturbance. 236 | 237 | The system output is: 238 | z = C*x 239 | 240 | The matrix Acl must be Hurwitz for the H2 norm to be finite. 241 | 242 | Parameters 243 | ---------- 244 | A : (n, n) Matrix, 245 | Input 246 | Bdisturbance : (n, m) Matrix 247 | Input 248 | C : (n, q) Matrix 249 | Input 250 | 251 | Returns 252 | ------- 253 | J2 : Systems H2 norm. 254 | ''' 255 | 256 | if not is_hurwitz(Acl): 257 | return np.inf 258 | 259 | #first, compute the controllability Gramian of (Acl, Bdisturbance) 260 | P = controllability_gramian(Acl, Bdisturbance) 261 | 262 | #output the gain 263 | return np.sqrt(np.trace(C.dot(P).dot(C.T))) 264 | 265 | 266 | def system_norm_Hinf(Acl, Bdisturbance, C, D = None, lowerBound = 0.0, upperBound = np.inf, relTolerance = 1e-3): 267 | '''Compute a system's Hinfinity norm. 268 | 269 | Acl, Bdisturbance are system matrices, describing the systems dynamics: 270 | dx/dt = Acl*x + Bdisturbance*v 271 | where x is the system state and v is the disturbance. 272 | 273 | The system output is: 274 | z = C*x + D*v 275 | 276 | The matrix Acl must be Hurwitz for the Hinf norm to be finite. 277 | 278 | The norm is found by iterating over the Riccati equation. The search can 279 | be sped up by providing lower and upper bounds for the norm. If ommitted, 280 | these are determined automatically. 281 | The search proceeds via bisection, and terminates when a specified relative 282 | tolerance is achieved. 283 | 284 | Parameters 285 | ---------- 286 | A : (n, n) Matrix 287 | Input 288 | Bdisturbance : (n, m) Matrix 289 | Input 290 | C : (q, n) Matrix 291 | Input 292 | D : (q,m) Matrix 293 | Input (optional) 294 | lowerBound: float 295 | Input (optional) 296 | upperBound: float 297 | Input (optional) 298 | relTolerance: float 299 | Input (optional) 300 | 301 | Returns 302 | ------- 303 | Jinf : Systems Hinf norm. 304 | 305 | ''' 306 | 307 | if not is_hurwitz(Acl): 308 | return np.inf 309 | 310 | eps = 1e-10 311 | 312 | if D is None: 313 | #construct a fake feed-through matrix 314 | D = np.matrix(np.zeros([C.shape[0], Bdisturbance.shape[1]])) 315 | 316 | 317 | def test_upper_bound(gamma, A, B, C, D): 318 | '''Is the given gamma an upper bound for the Hinf gain? 319 | ''' 320 | #Construct the R matrix: 321 | Rric = -gamma**2*np.matrix(np.eye(D.shape[1],D.shape[1])) + D.T.dot(D) 322 | #test that Rric is negative definite 323 | eigsR = np.linalg.eig(Rric)[0] 324 | if max(np.real(eigsR)) > -eps: 325 | return False, None 326 | 327 | #matrices for the Ricatti equation: 328 | Aric = A - B.dot(np.linalg.inv(Rric)).dot(D.T).dot(C) 329 | Bric = B 330 | Qric = C.T.dot(C) - C.T.dot(D).dot(np.linalg.inv(Rric)).dot(D.T).dot(C) 331 | 332 | try: 333 | X = scipy.linalg.solve_continuous_are(Aric, Bric, Qric, Rric) 334 | except np.linalg.linalg.LinAlgError: 335 | #Couldn't solve 336 | return False, None 337 | 338 | eigsX = np.linalg.eigvals(X) 339 | if (np.min(np.real(eigsX)) < 0) or (np.sum(np.abs(np.imag(eigsX)))>eps): 340 | #The ARE has to return a pos. semidefinite solution, but X is not 341 | return False, None 342 | if np.max(np.linalg.svd(X-X.T, compute_uv=False)) > 1e-6: 343 | #The ARE solution is not symmetric! Fail 344 | return False, None 345 | 346 | 347 | CL = A + B.dot(np.linalg.inv(-Rric)).dot(B.T.dot(X) + D.T.dot(C)) 348 | eigs = np.linalg.eigvals(CL) 349 | 350 | return (np.max(np.real(eigs)) < -eps), X 351 | 352 | #our output ricatti solution 353 | X = None 354 | 355 | #Are we supplied an upper bound? 356 | if not np.isfinite(upperBound): 357 | upperBound = max([1.0,lowerBound]) 358 | counter = 1 359 | while True: 360 | isOK, X2 = test_upper_bound(upperBound, Acl, Bdisturbance, C, D) 361 | 362 | if isOK: 363 | X = X2.copy() 364 | break 365 | 366 | upperBound *= 2.0 367 | counter += 1 368 | assert counter<1024, 'Exceeded max. number of iterations searching for upper bound' 369 | 370 | #perform a bisection search to find the gain: 371 | while (upperBound-lowerBound)>relTolerance*upperBound: 372 | g = 0.5*(upperBound+lowerBound) 373 | 374 | stab, X2 = test_upper_bound(g, Acl, Bdisturbance, C, D) 375 | if stab: 376 | upperBound = g 377 | X = X2 378 | else: 379 | lowerBound = g 380 | 381 | assert X is not None, 'No solution found! Check supplied upper bound' 382 | 383 | return upperBound 384 | 385 | 386 | 387 | def discretise_time(A, B, dt): 388 | '''Compute the exact discretization of the continuous system A,B. 389 | 390 | Goes from a description 391 | d/dt x(t) = A*x(t) + B*u(t) 392 | u(t) = ud[k] for t in [k*dt, (k+1)*dt) 393 | to the description 394 | xd[k+1] = Ad*xd[k] + Bd*ud[k] 395 | where 396 | xd[k] := x(k*dt) 397 | 398 | Returns: Ad, Bd 399 | ''' 400 | 401 | nstates = A.shape[0] 402 | ninputs = B.shape[1] 403 | 404 | M = np.matrix(np.zeros([nstates+ninputs,nstates+ninputs])) 405 | M[:nstates,:nstates] = A 406 | M[:nstates, nstates:] = B 407 | 408 | Md = scipy.linalg.expm(M*dt) 409 | Ad = Md[:nstates, :nstates] 410 | Bd = Md[:nstates, nstates:] 411 | 412 | return Ad, Bd 413 | 414 | 415 | 416 | 417 | 418 | -------------------------------------------------------------------------------- /controlpy/synthesis.py: -------------------------------------------------------------------------------- 1 | """ Tools for synthesising controllers for LTI systems. 2 | 3 | (c) 2014 Mark W. Mueller 4 | """ 5 | from __future__ import division, print_function 6 | 7 | import numpy as np 8 | import scipy.linalg 9 | 10 | from controlpy import analysis 11 | 12 | 13 | def controller_lqr(A, B, Q, R): 14 | """Solve the continuous time LQR controller for a continuous time system. 15 | 16 | A and B are system matrices, describing the systems dynamics: 17 | dx/dt = A x + B u 18 | 19 | The controller minimizes the infinite horizon quadratic cost function: 20 | cost = integral (x.T*Q*x + u.T*R*u) dt 21 | 22 | where Q is a positive semidefinite matrix, and R is positive definite matrix. 23 | 24 | Returns K, X, eigVals: 25 | Returns gain the optimal gain K, the solution matrix X, and the closed loop system eigenvalues. 26 | The optimal input is then computed as: 27 | input: u = -K*x 28 | """ 29 | #ref Bertsekas, p.151 30 | 31 | #first, try to solve the ricatti equation 32 | X = scipy.linalg.solve_continuous_are(A, B, Q, R) 33 | 34 | #compute the LQR gain 35 | K = np.dot(np.linalg.inv(R),(np.dot(B.T,X))) 36 | 37 | eigVals = np.linalg.eigvals(A-np.dot(B,K)) 38 | 39 | return K, X, eigVals 40 | 41 | 42 | 43 | def controller_lqr_discrete_time(A, B, Q, R): 44 | """Solve the discrete time LQR controller for a discrete time system. 45 | 46 | A and B are system matrices, describing the systems dynamics: 47 | x[k+1] = A x[k] + B u[k] 48 | 49 | The controller minimizes the infinite horizon quadratic cost function: 50 | cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] 51 | 52 | where Q is a positive semidefinite matrix, and R is positive definite matrix. 53 | 54 | Returns K, X, eigVals: 55 | Returns gain the optimal gain K, the solution matrix X, and the closed loop system eigenvalues. 56 | The optimal input is then computed as: 57 | input: u = -K*x 58 | """ 59 | 60 | #first, try to solve the ricatti equation 61 | X = scipy.linalg.solve_discrete_are(A, B, Q, R) 62 | 63 | #compute the LQR gain 64 | K = np.dot(np.linalg.inv(np.dot(np.dot(B.T,X),B)+R),(np.dot(np.dot(B.T,X),A))) 65 | 66 | eigVals = np.linalg.eigvals(A-np.dot(B,K)) 67 | 68 | return K, X, eigVals 69 | 70 | 71 | def estimator_kalman_steady_state_discrete_time(A, H, Q, R): 72 | """Solve the discrete time, steady-state Kalman filter for a discrete time system. 73 | 74 | A and H are system matrices, describing the systems dynamics: 75 | x[k+1] = A x[k] + B u[k] + v[k] 76 | z[k] = C x[k] + w[k] 77 | 78 | with v, w zero-mean noise sequences, and 79 | Var[v[k]] = Q 80 | Var[w[k]] = R 81 | 82 | and u[k] a known input 83 | 84 | Returns K, X, eigVals: 85 | Returns gain the optimal filter gain K, the solution matrix X, and the closed loop system eigenvalues. 86 | The estimate is then given by: 87 | est[k] = (I-K*H)A est[k-1] + (I-K*H)B u[k] + K meas[k] 88 | """ 89 | 90 | #first, try to solve the ricatti equation 91 | X = scipy.linalg.solve_discrete_are(A.T, H.T, Q, R) 92 | 93 | #compute the LQR gain 94 | K = X.dot(H.T).dot(np.linalg.inv(H.dot(X).dot(H.T)+R)) 95 | 96 | eigVals = np.linalg.eigvals( (np.identity(X.shape[0]) - K.dot(H)).dot(A) ) 97 | 98 | return K, X, eigVals 99 | 100 | 101 | def controller_lqr_discrete_from_continuous_time(A, B, Q, R, dt): 102 | """Solve the discrete time LQR controller for a continuous time system. 103 | 104 | A and B are system matrices, describing the systems dynamics: 105 | dx/dt = A x + B u 106 | 107 | The controller minimizes the infinite horizon quadratic cost function: 108 | cost = integral (x.T*Q*x + u.T*R*u) dt 109 | where Q is a positive semidefinite matrix, and R is positive definite matrix. 110 | 111 | The controller is implemented to run at discrete times, at a rate given by 112 | onboard_dt, 113 | i.e. u[k] = -K*x(k*t) 114 | Discretization is done by zero order hold. 115 | 116 | Returns K, X, eigVals: 117 | Returns gain the optimal gain K, the solution matrix X, and the closed loop system eigenvalues. 118 | The optimal input is then computed as: 119 | input: u = -K*x 120 | """ 121 | #ref Bertsekas, p.151 122 | 123 | Ad, Bd = analysis.discretise_time(A, B, dt) 124 | 125 | return controller_lqr_discrete_time(Ad, Bd, Q, R) 126 | 127 | 128 | 129 | def controller_H2_state_feedback(A, Binput, Bdist, C1, D12): 130 | """Solve for the optimal H2 static state feedback controller. 131 | 132 | A, Bdist, and Binput are system matrices, describing the systems dynamics: 133 | dx/dt = A*x + Binput*u + Bdist*v 134 | where x is the system state, u is the input, and v is the disturbance 135 | 136 | The goal is to minimize the output Z, defined as 137 | z = C1*x + D12*u 138 | 139 | The optimal output is given by a static feedback gain: 140 | u = - K*x 141 | 142 | This is related to the LQR problem, where the state cost matrix is Q and 143 | the input cost matrix is R, then: 144 | C1 = [[sqrt(Q)], [0]] and D = [[0], [sqrt(D12)]] 145 | With sqrt(Q).T*sqrt(Q) = Q 146 | 147 | Parameters 148 | ---------- 149 | A : (n, n) Matrix 150 | Input 151 | Bdist : (n, m) Matrix 152 | Input 153 | Binput : (n, p) Matrix 154 | Input 155 | C1 : (q, n) Matrix 156 | Input 157 | D12: (q, p) Matrix 158 | Input 159 | 160 | Returns 161 | ------- 162 | K : (m, n) Matrix 163 | H2 optimal controller gain 164 | X : (n, n) Matrix 165 | Solution to the Ricatti equation 166 | J : Minimum H2 cost value 167 | 168 | """ 169 | 170 | X = scipy.linalg.solve_continuous_are(A, Binput, C1.T.dot(C1), D12.T.dot(D12)) 171 | 172 | K = np.linalg.inv(D12.T.dot(D12)).dot(Binput.T).dot(X) 173 | 174 | J = np.sqrt(np.trace(Bdist.T.dot(X.dot(Bdist)))) 175 | 176 | return K, X, J 177 | 178 | 179 | def observer_H2(A, Bdist, C1, C2, D21): 180 | """Solve for the optimal H2 state observer. 181 | 182 | TODO: document this! 183 | 184 | """ 185 | 186 | return controller_H2_state_feedback(A.T, C2.T, C1.T, Bdist.T, D21.T) 187 | 188 | 189 | # def controller_H2_output_feedback(A, Binput, Bdist, C1, D12, C2, D21): 190 | # """Solve for the optimal H2 output feedback controller. 191 | # 192 | # TODO: document this! 193 | # 194 | # u = K*xhat 195 | # d/dt xhat = A*xhat - Binput*u + L*(y - C2*xhat) 196 | # 197 | # """ 198 | # 199 | # K, X, Jc = controller_H2_state_feedback(A, Binput, Bdist, C1, D12) 200 | # L, S, Jo = observer_H2(A, Bdist, C1, C2, D21) 201 | # 202 | # J = np.sqrt(Jc**2 + Jo**2) 203 | # 204 | # return K, L, X, S, (Jc, Jo, J) 205 | 206 | 207 | def controller_Hinf_state_feedback(A, Binput, Bdist, C1, D12, stabilityBoundaryEps=1e-16, gammaRelTol=1e-3, gammaLB = 0, gammaUB = np.Inf, subOptimality = 1.0): 208 | """Solve for the optimal H_infinity static state feedback controller. 209 | 210 | A, Bdist, and Binput are system matrices, describing the systems dynamics: 211 | dx/dt = A*x + Binput*u + Bdist*v 212 | where x is the system state, u is the input, and v is the disturbance 213 | 214 | The goal is to minimize the output Z, in the H_inf sense, defined as 215 | z = C1*x + D12*u 216 | 217 | The optimal output is given by a static feedback gain: 218 | u = - K*x 219 | 220 | The optimizing gain is found by a bisection search to within a relative 221 | tolerance gammaRelTol, which may be supplied by the user. The search may 222 | be initialised with lower and upper bounds gammaLB and gammaUB. 223 | 224 | The user may also specify a desired suboptimality, so that the returned 225 | controller does not achieve the minimum Hinf norm. This may be desirable 226 | because of numerical issues near the optimal solution. 227 | 228 | Parameters 229 | ---------- 230 | A : (n, n) Matrix 231 | Input 232 | Bdist : (n, m) Matrix 233 | Input 234 | Binput : (n, p) Matrix 235 | Input 236 | C1 : (q, n) Matrix 237 | Input 238 | D12: (q, p) Matrix 239 | Input 240 | stabilityBoundaryEps: float 241 | Input (optional) 242 | gammaRelTol: float 243 | Input (optional) 244 | gammaLB: float 245 | Input (optional) 246 | gammaUB: float 247 | Input (optional) 248 | 249 | Returns 250 | ------- 251 | K : (m, n) Matrix 252 | Hinf optimal controller gain 253 | X : (n, n) Matrix 254 | Solution to the Ricatti equation 255 | J : Minimum cost value (gamma) 256 | """ 257 | 258 | assert analysis.is_stabilisable(A, Binput), '(A, Binput) must be stabilisable' 259 | assert np.linalg.det(D12.T.dot(D12)), 'D12.T*D12 must be invertible' 260 | assert np.max(np.abs(D12.T.dot(C1)))==0, 'D12.T*C1 must be zero' 261 | tmp = analysis.unobservable_modes(C1, A, returnEigenValues=True)[1] 262 | if tmp: 263 | assert np.max(np.abs(np.real(tmp)))>0, 'The pair (C1,A) must have no unobservable modes on imag. axis' 264 | 265 | #First, solve the ARE: 266 | # A.T*X+X*A - X*Binput*inv(D12.T*D12)*Binput.T*X + gamma**(-2)*X*Bdist*Bdist.T*X + C1.T*C1 = 0 267 | #Let: 268 | # R = [[-gamma**(-2)*eye, 0],[0, D12.T*D12]] 269 | # B = [Bdist, Binput] 270 | # Q = C1.T*C1 271 | #then we have to solve 272 | # A.T*X+X*A - X*B*inv(R)*B.T*X + Q = 0 273 | 274 | B = np.matrix(np.zeros([Bdist.shape[0],(Bdist.shape[1]+Binput.shape[1])])) 275 | B[:,:Bdist.shape[1]] = Bdist 276 | B[:,Bdist.shape[1]:] = Binput 277 | 278 | R = np.matrix(np.zeros([B.shape[1], B.shape[1]])) 279 | #we fill the upper left of R later. 280 | R[Bdist.shape[1]:,Bdist.shape[1]:] = D12.T.dot(D12) 281 | Q = C1.T.dot(C1) 282 | 283 | #Define a helper function: 284 | def has_stable_solution(g, A, B, Q, R, eps): 285 | R[0:Bdist.shape[1], 0:Bdist.shape[1]] = -g**(2)*np.eye(Bdist.shape[1], Bdist.shape[1]) 286 | 287 | #Riccati equation prerequisites: 288 | if not analysis.is_stabilisable(A, B): 289 | return False, None 290 | 291 | if not analysis.is_detectable(Q, A): 292 | return False, None 293 | 294 | try: 295 | X = scipy.linalg.solve_continuous_are(A, B, Q, R) 296 | except np.linalg.linalg.LinAlgError: 297 | return False, None 298 | 299 | eigsX = np.linalg.eigvals(X) 300 | 301 | if (np.min(np.real(eigsX)) < 0) or (np.sum(np.abs(np.imag(eigsX)))>eps): 302 | #The ARE has to return a pos. semidefinite solution, but X is not 303 | return False, None 304 | 305 | 306 | CL = A - Binput.dot(np.linalg.inv(D12.T.dot(D12))).dot(Binput.T).dot(X) + g**(-2)*Bdist.dot(Bdist.T).dot(X) 307 | eigs = np.linalg.eigvals(CL) 308 | 309 | return (np.max(np.real(eigs)) < -eps), X 310 | 311 | 312 | X = None 313 | if np.isinf(gammaUB): 314 | #automatically choose an UB 315 | gammaUB = np.float(np.max([1, gammaLB])) 316 | 317 | #Find an upper bound: 318 | counter = 1 319 | while True: 320 | 321 | stab, X2 = has_stable_solution(gammaUB, A, B, Q, R, stabilityBoundaryEps) 322 | if stab: 323 | X = X2.copy() 324 | break 325 | 326 | gammaUB *= 2 327 | counter += 1 328 | 329 | assert counter < 1024, 'Exceeded max number of iterations searching for upper gamma bound!' 330 | 331 | #Find the minimising gain 332 | while (gammaUB-gammaLB)>gammaRelTol*gammaUB: 333 | g = 0.5*(gammaUB+gammaLB) 334 | 335 | stab, X2 = has_stable_solution(g, A, B, Q, R, stabilityBoundaryEps) 336 | if stab: 337 | gammaUB = g 338 | X = X2 339 | else: 340 | gammaLB = g 341 | 342 | assert X is not None, 'No solution found! Check supplied upper bound' 343 | 344 | g = gammaUB 345 | if subOptimality > 1.0: 346 | #compute a sub optimal solution 347 | g *= subOptimality 348 | stab, X = has_stable_solution(g, A, B, Q, R, stabilityBoundaryEps) 349 | assert stab, 'Sub-optimal solution not found!' 350 | 351 | K = np.linalg.inv(D12.T.dot(D12)).dot(Binput.T).dot(X) 352 | 353 | J = g 354 | return K, X, J 355 | 356 | 357 | -------------------------------------------------------------------------------- /examples/example_controllability.py: -------------------------------------------------------------------------------- 1 | '''Example showing how to test a system's controllability 2 | ''' 3 | 4 | from __future__ import print_function, division 5 | 6 | import controlpy 7 | 8 | import numpy as np 9 | 10 | # A single input system, one uncontrollable mode. 11 | A = np.matrix([[0,1,0],[0,0,1],[0,0,5]]) 12 | B = np.matrix([[0],[1],[0]]) 13 | 14 | uncontrollableModes = controlpy.analysis.uncontrollable_modes(A,B) 15 | 16 | if not uncontrollableModes: 17 | print('System is controllable.') 18 | else: 19 | print('System is uncontrollable. Uncontrollable modes are:') 20 | print(uncontrollableModes) 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /examples/example_lqr.py: -------------------------------------------------------------------------------- 1 | '''A simple example script, which implements an LQR controller for a double integrator. 2 | ''' 3 | 4 | from __future__ import print_function, division 5 | 6 | import controlpy 7 | 8 | import numpy as np 9 | 10 | # Example system is a double integrator: 11 | A = np.matrix([[0,1],[0,0]]) 12 | B = np.matrix([[0],[1]]) 13 | 14 | # Define our costs: 15 | Q = np.matrix([[1,0],[0,0]]) 16 | R = np.matrix([[1]]) 17 | 18 | # Compute the LQR controller 19 | gain, X, closedLoopEigVals = controlpy.synthesis.controller_lqr(A,B,Q,R) 20 | 21 | print('The computed gain is:') 22 | print(gain) 23 | 24 | print('The closed loop eigenvalues are:') 25 | print(closedLoopEigVals) 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /examples/example_systemGain.py: -------------------------------------------------------------------------------- 1 | '''A simple example script, compute the norm of an LTI system 2 | ''' 3 | 4 | from __future__ import print_function, division 5 | 6 | import controlpy 7 | 8 | import numpy as np 9 | 10 | # The system is 11 | # dx = A*x + B*v 12 | # z = C*x 13 | # v is a disturbance. 14 | 15 | A = np.matrix([[-0.1, 10],[0,-0.7]]) 16 | B = np.matrix([[0],[1]]) 17 | C = np.matrix([[1,0]]) 18 | 19 | print('H2 norm: ', controlpy.analysis.system_norm_H2(A, B, C)) 20 | print('Hinf norm: ', controlpy.analysis.system_norm_Hinf(A, B, C)) 21 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | description-file = README.md 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | def readme(): 4 | with open('README.rst') as f: 5 | return f.read() 6 | 7 | setup(name='controlpy', 8 | version='0.1.1', 9 | description='Python control library', 10 | long_description='Tools for analysing and synthesising controllers in python. Includes LQR solvers, methods for H2 and Hinf, and other potentially useful (state space) methods.', 11 | url='http://github.com/markwmuller/controlpy', 12 | download_url = 'https://github.com/markwmuller/controlpy/archive/0.1', 13 | author='Mark W. Mueller', 14 | author_email='mwm@mwm.im', 15 | license='GPL V3', 16 | packages=['controlpy'], 17 | zip_safe=False, 18 | classifiers=[], 19 | install_requires=['numpy','scipy'], 20 | keywords='control lqr robust H2 Hinf Hinfinity', 21 | tests_require=['cvxpy'], 22 | ) 23 | -------------------------------------------------------------------------------- /tests/test_analysis.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function, division 2 | 3 | import os 4 | import sys 5 | sys.path.insert(0, os.path.abspath('..')) 6 | 7 | import numpy as np 8 | 9 | np.random.seed(1234) 10 | 11 | import controlpy 12 | 13 | import unittest 14 | 15 | import cvxpy 16 | def sys_norm_h2_LMI(Acl, Bdisturbance, C): 17 | #doesn't work very well, if problem poorly scaled Riccati works better. 18 | #Dullerud p 210 19 | n = Acl.shape[0] 20 | X = cvxpy.Semidef(n) 21 | Y = cvxpy.Semidef(n) 22 | 23 | constraints = [ Acl*X + X*Acl.T + Bdisturbance*Bdisturbance.T == -Y, 24 | ] 25 | 26 | obj = cvxpy.Minimize(cvxpy.trace(Y)) 27 | 28 | prob = cvxpy.Problem(obj, constraints) 29 | 30 | prob.solve() 31 | eps = 1e-16 32 | if np.max(np.linalg.eigvals((-Acl*X - X*Acl.T - Bdisturbance*Bdisturbance.T).value)) > -eps: 33 | print('Acl*X + X*Acl.T +Bdisturbance*Bdisturbance.T is not neg def.') 34 | return np.Inf 35 | 36 | if np.min(np.linalg.eigvals(X.value)) < eps: 37 | print('X is not pos def.') 38 | return np.Inf 39 | 40 | return np.sqrt(np.trace(C*X.value*C.T)) 41 | 42 | 43 | def sys_norm_hinf_LMI(A, Bdisturbance, C, D = None): 44 | '''Compute a system's Hinfinity norm, using an LMI approach. 45 | 46 | Acl, Bdisturbance are system matrices, describing the systems dynamics: 47 | dx/dt = Acl*x + Bdisturbance*v 48 | where x is the system state and v is the disturbance. 49 | 50 | The system output is: 51 | z = C*x + D*v 52 | 53 | The matrix Acl must be Hurwitz for the Hinf norm to be finite. 54 | 55 | Parameters 56 | ---------- 57 | A : (n, n) Matrix 58 | Input 59 | Bdisturbance : (n, m) Matrix 60 | Input 61 | C : (q, n) Matrix 62 | Input 63 | D : (q,m) Matrix 64 | Input (optional) 65 | 66 | Returns 67 | ------- 68 | Jinf : Systems Hinf norm. 69 | 70 | See: Robust Controller Design By Convex Optimization, Alireza Karimi Laboratoire d'Automatique, EPFL 71 | ''' 72 | 73 | if not controlpy.analysis.is_hurwitz(A): 74 | return np.Inf 75 | 76 | n = A.shape[0] 77 | ndist = Bdisturbance.shape[1] 78 | nout = C.shape[0] 79 | 80 | X = cvxpy.Semidef(n) 81 | g = cvxpy.Variable() 82 | 83 | if D is None: 84 | D = np.matrix(np.zeros([nout, ndist])) 85 | 86 | r1 = cvxpy.hstack(cvxpy.hstack(A.T*X+X*A, X*Bdisturbance), C.T) 87 | r2 = cvxpy.hstack(cvxpy.hstack(Bdisturbance.T*X, -g*np.matrix(np.identity(ndist))), D.T) 88 | r3 = cvxpy.hstack(cvxpy.hstack(C, D), -g*np.matrix(np.identity(nout))) 89 | tmp = cvxpy.vstack(cvxpy.vstack(r1,r2),r3) 90 | 91 | constraints = [tmp == -cvxpy.Semidef(n + ndist + nout)] 92 | 93 | obj = cvxpy.Minimize(g) 94 | 95 | prob = cvxpy.Problem(obj, constraints) 96 | 97 | try: 98 | prob.solve()#solver='CVXOPT', kktsolver='robust') 99 | except cvxpy.error.SolverError: 100 | print('Solution not found!') 101 | return None 102 | 103 | if not prob.status == cvxpy.OPTIMAL: 104 | return None 105 | 106 | return g.value 107 | 108 | 109 | 110 | class TestAnalysis(unittest.TestCase): 111 | 112 | def test_scalar_hurwitz(self): 113 | self.assertTrue(controlpy.analysis.is_hurwitz(np.matrix([[-1]]))) 114 | self.assertTrue(controlpy.analysis.is_hurwitz(np.array([[-1]]))) 115 | self.assertFalse(controlpy.analysis.is_hurwitz(np.matrix([[1]]))) 116 | self.assertFalse(controlpy.analysis.is_hurwitz(np.array([[1]]))) 117 | 118 | 119 | def test_multidim_hurwitz(self): 120 | for matType in [np.matrix, np.array]: 121 | Astable = matType([[-1,2,1000], [0,-2,3], [0,0,-9]]) 122 | Aunstable = matType([[1,2,1000], [0,-2,3], [0,0,-9]]) 123 | Acritical = matType([[0,2,10], [0,-2,3], [0,0,-9]]) 124 | 125 | tolerance = -1e-6 # for critically stable case 126 | 127 | for i in range(1000): 128 | T = matType(np.random.normal(size=[3,3])) 129 | Tinv = np.linalg.inv(T) 130 | 131 | Bs = np.dot(np.dot(T,Astable),Tinv) 132 | Bu = np.dot(np.dot(T,Aunstable),Tinv) 133 | Bc = np.dot(np.dot(T,Acritical),Tinv) 134 | 135 | self.assertTrue(controlpy.analysis.is_hurwitz(Bs), str(i)) 136 | self.assertFalse(controlpy.analysis.is_hurwitz(Bu), str(i)) 137 | 138 | self.assertFalse(controlpy.analysis.is_hurwitz(Bc, tolerance=tolerance), 'XXX'+str(i)+'_'+str(np.max(np.real(np.linalg.eigvals(Bc))))) 139 | 140 | 141 | # def test_uncontrollable_modes(self): 142 | # #TODO: FIXME: THIS FAILS! 143 | # for matType in [np.matrix, np.array]: 144 | # 145 | # es = [-1,0,1] 146 | # for e in es: 147 | # A = matType([[e,2,0],[0,e,0],[0,0,e]]) 148 | # B = matType([[0,1,0]]).T 149 | # 150 | # tolerance = 1e-9 151 | # for i in range(1000): 152 | # T = matType(np.random.normal(size=[3,3])) 153 | # Tinv = np.linalg.inv(T) 154 | # 155 | # AA = np.dot(np.dot(T,A),Tinv) 156 | # BB = np.dot(T,B) 157 | # 158 | # isControllable = controlpy.analysis.is_controllable(AA, BB, tolerance=tolerance) 159 | # self.assertFalse(isControllable) 160 | # 161 | # isStabilisable = controlpy.analysis.is_stabilisable(A, B) 162 | # self.assertEqual(isStabilisable, e<0) 163 | # 164 | # if 0: 165 | # #These shouldn't fail! 166 | # uncontrollableModes, uncontrollableEigenValues = controlpy.analysis.uncontrollable_modes(AA, BB, returnEigenValues=True, tolerance=tolerance) 167 | # self.assertEqual(len(uncontrollableModes), 1) 168 | # 169 | # self.assertAlmostEqual(uncontrollableEigenValues[0], 1, delta=tolerance) 170 | # self.assertAlmostEqual(np.linalg.norm(uncontrollableModes[0] - np.matrix([[0,0,1]]).T), 0, delta=tolerance) 171 | 172 | 173 | 174 | def test_time_discretisation(self): 175 | for matType in [np.matrix, np.array]: 176 | Ac = matType([[0,1],[0,0]]) 177 | Bc = matType([[0],[1]]) 178 | 179 | dt = 0.1 180 | 181 | Ad = matType([[1,dt],[0,1]]) 182 | Bd = matType([[dt**2/2],[dt]]) 183 | for i in range(1000): 184 | T = matType(np.random.normal(size=[2,2])) 185 | Tinv = np.linalg.inv(T) 186 | 187 | AAc = np.dot(np.dot(T,Ac),Tinv) 188 | BBc = np.dot(T,Bc) 189 | 190 | AAd = np.dot(np.dot(T,Ad),Tinv) 191 | BBd = np.dot(T,Bd) 192 | 193 | AAd2, BBd2 = controlpy.analysis.discretise_time(AAc, BBc, dt) 194 | 195 | self.assertLess(np.linalg.norm(AAd-AAd2), 1e-6) 196 | self.assertLess(np.linalg.norm(BBd-BBd2), 1e-6) 197 | 198 | 199 | #test some random systems against Euler discretisation 200 | nx = 20 201 | nu = 20 202 | dt = 1e-6 203 | tol = 1e-3 204 | for i in range(1000): 205 | Ac = matType(np.random.normal(size=[nx,nx])) 206 | Bc = matType(np.random.normal(size=[nx,nu])) 207 | 208 | Ad1 = np.identity(nx)+Ac*dt 209 | Bd1 = Bc*dt 210 | 211 | Ad2, Bd2 = controlpy.analysis.discretise_time(Ac, Bc, dt) 212 | 213 | self.assertLess(np.linalg.norm(Ad1-Ad2), tol) 214 | self.assertLess(np.linalg.norm(Bd1-Bd2), tol) 215 | 216 | 217 | 218 | def test_system_norm_H2(self): 219 | for matType in [np.matrix, np.array]: 220 | A = matType([[-1,2],[0,-3]]) 221 | B = matType([[0,1]]).T 222 | C = matType([[1,0]]) 223 | 224 | h2norm = controlpy.analysis.system_norm_H2(A, B, C) 225 | 226 | h2norm_lmi = sys_norm_h2_LMI(A, B, C) 227 | 228 | tol = 1e-3 229 | self.assertLess(np.linalg.norm(h2norm-h2norm_lmi), tol) 230 | 231 | 232 | def test_system_norm_Hinf(self): 233 | for matType in [np.matrix, np.array]: 234 | A = matType([[-1,2],[0,-3]]) 235 | B = matType([[0,1]]).T 236 | C = matType([[1,0]]) 237 | 238 | hinfnorm = controlpy.analysis.system_norm_Hinf(A, B, C) 239 | 240 | hinfnorm_lmi = sys_norm_hinf_LMI(A, B, C) 241 | 242 | tol = 1e-3 243 | self.assertLess(np.linalg.norm(hinfnorm-hinfnorm_lmi), tol) 244 | 245 | #TODO: 246 | # - observability tests, similar to controllability 247 | 248 | 249 | 250 | 251 | if __name__ == '__main__': 252 | np.random.seed(1) 253 | unittest.main() 254 | -------------------------------------------------------------------------------- /tests/test_synth.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function, division 2 | 3 | import os 4 | import sys 5 | sys.path.insert(0, os.path.abspath('..')) 6 | 7 | import numpy as np 8 | 9 | np.random.seed(1234) 10 | 11 | import controlpy 12 | 13 | import unittest 14 | 15 | import cvxpy 16 | def synth_h2_state_feedback_LMI(A, Binput, Bdist, C1, D12): 17 | #Dullerud p 217 (?) 18 | 19 | n = A.shape[0] #num states 20 | m = Binput.shape[1] #num control inputs 21 | q = C1.shape[0] #num outputs to "be kept small" 22 | 23 | X = cvxpy.Variable(n,n) 24 | Y = cvxpy.Variable(m,n) 25 | Z = cvxpy.Variable(q,q) 26 | 27 | tmp1 = cvxpy.hstack(X, (C1*X+D12*Y).T) 28 | tmp2 = cvxpy.hstack((C1*X+D12*Y), Z) 29 | tmp = cvxpy.vstack(tmp1, tmp2) 30 | 31 | constraints = [A*X + Binput*Y + X*A.T + Y.T*Binput.T + Bdist*Bdist.T == -cvxpy.Semidef(n), 32 | tmp == cvxpy.Semidef(n+q), 33 | ] 34 | 35 | obj = cvxpy.Minimize(cvxpy.trace(Z)) 36 | 37 | prob = cvxpy.Problem(obj, constraints) 38 | 39 | prob.solve(solver='CVXOPT', kktsolver='robust') 40 | 41 | K = -Y.value*np.linalg.inv(X.value) 42 | return K 43 | 44 | 45 | class TestSynthesis(unittest.TestCase): 46 | 47 | def test_h2(self): 48 | for matType in [np.matrix, np.array]: 49 | A = matType([[1,2],[0,3]]) 50 | B = matType([[0,1]]).T 51 | Bdist = matType([[0,1]]).T 52 | C1 = matType([[1,0],[0,1],[0,0]]) 53 | D12= matType([[0,0,1]]).T 54 | 55 | KLMI = synth_h2_state_feedback_LMI(A, B, Bdist, C1, D12) 56 | K2, X2, J2 = controlpy.synthesis.controller_H2_state_feedback(A, B, Bdist, C1, D12) 57 | 58 | self.assertLess(np.linalg.norm(K2-KLMI), 1e-3*np.linalg.norm(K2)) #note sign difference 59 | 60 | 61 | 62 | 63 | if __name__ == '__main__': 64 | np.random.seed(1) 65 | unittest.main() 66 | --------------------------------------------------------------------------------