1184 lines
60 KiB
HTML
1184 lines
60 KiB
HTML
|
<?xml version="1.0" encoding="utf-8"?>
|
||
|
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
||
|
|
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||
|
|
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||
|
|
<head>
|
||
|
|
<!-- 2019-08-14 mer. 12:08 -->
|
||
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||
|
|
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||
|
|
<title>On the Design of Complementary Filters for Control - Computation with Matlab</title>
|
||
|
|
<meta name="generator" content="Org mode" />
|
||
|
|
<meta name="author" content="Thomas Dehaeze" />
|
||
|
|
<style type="text/css">
|
||
|
|
<!--/*--><![CDATA[/*><!--*/
|
||
|
|
.title { text-align: center;
|
||
|
|
margin-bottom: .2em; }
|
||
|
|
.subtitle { text-align: center;
|
||
|
|
font-size: medium;
|
||
|
|
font-weight: bold;
|
||
|
|
margin-top:0; }
|
||
|
|
.todo { font-family: monospace; color: red; }
|
||
|
|
.done { font-family: monospace; color: green; }
|
||
|
|
.priority { font-family: monospace; color: orange; }
|
||
|
|
.tag { background-color: #eee; font-family: monospace;
|
||
|
|
padding: 2px; font-size: 80%; font-weight: normal; }
|
||
|
|
.timestamp { color: #bebebe; }
|
||
|
|
.timestamp-kwd { color: #5f9ea0; }
|
||
|
|
.org-right { margin-left: auto; margin-right: 0px; text-align: right; }
|
||
|
|
.org-left { margin-left: 0px; margin-right: auto; text-align: left; }
|
||
|
|
.org-center { margin-left: auto; margin-right: auto; text-align: center; }
|
||
|
|
.underline { text-decoration: underline; }
|
||
|
|
#postamble p, #preamble p { font-size: 90%; margin: .2em; }
|
||
|
|
p.verse { margin-left: 3%; }
|
||
|
|
pre {
|
||
|
|
border: 1px solid #ccc;
|
||
|
|
box-shadow: 3px 3px 3px #eee;
|
||
|
|
padding: 8pt;
|
||
|
|
font-family: monospace;
|
||
|
|
overflow: auto;
|
||
|
|
margin: 1.2em;
|
||
|
|
}
|
||
|
|
pre.src {
|
||
|
|
position: relative;
|
||
|
|
overflow: visible;
|
||
|
|
padding-top: 1.2em;
|
||
|
|
}
|
||
|
|
pre.src:before {
|
||
|
|
display: none;
|
||
|
|
position: absolute;
|
||
|
|
background-color: white;
|
||
|
|
top: -10px;
|
||
|
|
right: 10px;
|
||
|
|
padding: 3px;
|
||
|
|
border: 1px solid black;
|
||
|
|
}
|
||
|
|
pre.src:hover:before { display: inline;}
|
||
|
|
/* Languages per Org manual */
|
||
|
|
pre.src-asymptote:before { content: 'Asymptote'; }
|
||
|
|
pre.src-awk:before { content: 'Awk'; }
|
||
|
|
pre.src-C:before { content: 'C'; }
|
||
|
|
/* pre.src-C++ doesn't work in CSS */
|
||
|
|
pre.src-clojure:before { content: 'Clojure'; }
|
||
|
|
pre.src-css:before { content: 'CSS'; }
|
||
|
|
pre.src-D:before { content: 'D'; }
|
||
|
|
pre.src-ditaa:before { content: 'ditaa'; }
|
||
|
|
pre.src-dot:before { content: 'Graphviz'; }
|
||
|
|
pre.src-calc:before { content: 'Emacs Calc'; }
|
||
|
|
pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
|
||
|
|
pre.src-fortran:before { content: 'Fortran'; }
|
||
|
|
pre.src-gnuplot:before { content: 'gnuplot'; }
|
||
|
|
pre.src-haskell:before { content: 'Haskell'; }
|
||
|
|
pre.src-hledger:before { content: 'hledger'; }
|
||
|
|
pre.src-java:before { content: 'Java'; }
|
||
|
|
pre.src-js:before { content: 'Javascript'; }
|
||
|
|
pre.src-latex:before { content: 'LaTeX'; }
|
||
|
|
pre.src-ledger:before { content: 'Ledger'; }
|
||
|
|
pre.src-lisp:before { content: 'Lisp'; }
|
||
|
|
pre.src-lilypond:before { content: 'Lilypond'; }
|
||
|
|
pre.src-lua:before { content: 'Lua'; }
|
||
|
|
pre.src-matlab:before { content: 'MATLAB'; }
|
||
|
|
pre.src-mscgen:before { content: 'Mscgen'; }
|
||
|
|
pre.src-ocaml:before { content: 'Objective Caml'; }
|
||
|
|
pre.src-octave:before { content: 'Octave'; }
|
||
|
|
pre.src-org:before { content: 'Org mode'; }
|
||
|
|
pre.src-oz:before { content: 'OZ'; }
|
||
|
|
pre.src-plantuml:before { content: 'Plantuml'; }
|
||
|
|
pre.src-processing:before { content: 'Processing.js'; }
|
||
|
|
pre.src-python:before { content: 'Python'; }
|
||
|
|
pre.src-R:before { content: 'R'; }
|
||
|
|
pre.src-ruby:before { content: 'Ruby'; }
|
||
|
|
pre.src-sass:before { content: 'Sass'; }
|
||
|
|
pre.src-scheme:before { content: 'Scheme'; }
|
||
|
|
pre.src-screen:before { content: 'Gnu Screen'; }
|
||
|
|
pre.src-sed:before { content: 'Sed'; }
|
||
|
|
pre.src-sh:before { content: 'shell'; }
|
||
|
|
pre.src-sql:before { content: 'SQL'; }
|
||
|
|
pre.src-sqlite:before { content: 'SQLite'; }
|
||
|
|
/* additional languages in org.el's org-babel-load-languages alist */
|
||
|
|
pre.src-forth:before { content: 'Forth'; }
|
||
|
|
pre.src-io:before { content: 'IO'; }
|
||
|
|
pre.src-J:before { content: 'J'; }
|
||
|
|
pre.src-makefile:before { content: 'Makefile'; }
|
||
|
|
pre.src-maxima:before { content: 'Maxima'; }
|
||
|
|
pre.src-perl:before { content: 'Perl'; }
|
||
|
|
pre.src-picolisp:before { content: 'Pico Lisp'; }
|
||
|
|
pre.src-scala:before { content: 'Scala'; }
|
||
|
|
pre.src-shell:before { content: 'Shell Script'; }
|
||
|
|
pre.src-ebnf2ps:before { content: 'ebfn2ps'; }
|
||
|
|
/* additional language identifiers per "defun org-babel-execute"
|
||
|
|
in ob-*.el */
|
||
|
|
pre.src-cpp:before { content: 'C++'; }
|
||
|
|
pre.src-abc:before { content: 'ABC'; }
|
||
|
|
pre.src-coq:before { content: 'Coq'; }
|
||
|
|
pre.src-groovy:before { content: 'Groovy'; }
|
||
|
|
/* additional language identifiers from org-babel-shell-names in
|
||
|
|
ob-shell.el: ob-shell is the only babel language using a lambda to put
|
||
|
|
the execution function name together. */
|
||
|
|
pre.src-bash:before { content: 'bash'; }
|
||
|
|
pre.src-csh:before { content: 'csh'; }
|
||
|
|
pre.src-ash:before { content: 'ash'; }
|
||
|
|
pre.src-dash:before { content: 'dash'; }
|
||
|
|
pre.src-ksh:before { content: 'ksh'; }
|
||
|
|
pre.src-mksh:before { content: 'mksh'; }
|
||
|
|
pre.src-posh:before { content: 'posh'; }
|
||
|
|
/* Additional Emacs modes also supported by the LaTeX listings package */
|
||
|
|
pre.src-ada:before { content: 'Ada'; }
|
||
|
|
pre.src-asm:before { content: 'Assembler'; }
|
||
|
|
pre.src-caml:before { content: 'Caml'; }
|
||
|
|
pre.src-delphi:before { content: 'Delphi'; }
|
||
|
|
pre.src-html:before { content: 'HTML'; }
|
||
|
|
pre.src-idl:before { content: 'IDL'; }
|
||
|
|
pre.src-mercury:before { content: 'Mercury'; }
|
||
|
|
pre.src-metapost:before { content: 'MetaPost'; }
|
||
|
|
pre.src-modula-2:before { content: 'Modula-2'; }
|
||
|
|
pre.src-pascal:before { content: 'Pascal'; }
|
||
|
|
pre.src-ps:before { content: 'PostScript'; }
|
||
|
|
pre.src-prolog:before { content: 'Prolog'; }
|
||
|
|
pre.src-simula:before { content: 'Simula'; }
|
||
|
|
pre.src-tcl:before { content: 'tcl'; }
|
||
|
|
pre.src-tex:before { content: 'TeX'; }
|
||
|
|
pre.src-plain-tex:before { content: 'Plain TeX'; }
|
||
|
|
pre.src-verilog:before { content: 'Verilog'; }
|
||
|
|
pre.src-vhdl:before { content: 'VHDL'; }
|
||
|
|
pre.src-xml:before { content: 'XML'; }
|
||
|
|
pre.src-nxml:before { content: 'XML'; }
|
||
|
|
/* add a generic configuration mode; LaTeX export needs an additional
|
||
|
|
(add-to-list 'org-latex-listings-langs '(conf " ")) in .emacs */
|
||
|
|
pre.src-conf:before { content: 'Configuration File'; }
|
||
|
|
|
||
|
|
table { border-collapse:collapse; }
|
||
|
|
caption.t-above { caption-side: top; }
|
||
|
|
caption.t-bottom { caption-side: bottom; }
|
||
|
|
td, th { vertical-align:top; }
|
||
|
|
th.org-right { text-align: center; }
|
||
|
|
th.org-left { text-align: center; }
|
||
|
|
th.org-center { text-align: center; }
|
||
|
|
td.org-right { text-align: right; }
|
||
|
|
td.org-left { text-align: left; }
|
||
|
|
td.org-center { text-align: center; }
|
||
|
|
dt { font-weight: bold; }
|
||
|
|
.footpara { display: inline; }
|
||
|
|
.footdef { margin-bottom: 1em; }
|
||
|
|
.figure { padding: 1em; }
|
||
|
|
.figure p { text-align: center; }
|
||
|
|
.equation-container {
|
||
|
|
display: table;
|
||
|
|
text-align: center;
|
||
|
|
width: 100%;
|
||
|
|
}
|
||
|
|
.equation {
|
||
|
|
vertical-align: middle;
|
||
|
|
}
|
||
|
|
.equation-label {
|
||
|
|
display: table-cell;
|
||
|
|
text-align: right;
|
||
|
|
vertical-align: middle;
|
||
|
|
}
|
||
|
|
.inlinetask {
|
||
|
|
padding: 10px;
|
||
|
|
border: 2px solid gray;
|
||
|
|
margin: 10px;
|
||
|
|
background: #ffffcc;
|
||
|
|
}
|
||
|
|
#org-div-home-and-up
|
||
|
|
{ text-align: right; font-size: 70%; white-space: nowrap; }
|
||
|
|
textarea { overflow-x: auto; }
|
||
|
|
.linenr { font-size: smaller }
|
||
|
|
.code-highlighted { background-color: #ffff00; }
|
||
|
|
.org-info-js_info-navigation { border-style: none; }
|
||
|
|
#org-info-js_console-label
|
||
|
|
{ font-size: 10px; font-weight: bold; white-space: nowrap; }
|
||
|
|
.org-info-js_search-highlight
|
||
|
|
{ background-color: #ffff00; color: #000000; font-weight: bold; }
|
||
|
|
.org-svg { width: 90%; }
|
||
|
|
/*]]>*/-->
|
||
|
|
</style>
|
||
|
|
<link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||
|
|
<link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||
|
|
<script src="../js/jquery.min.js"></script>
|
||
|
|
<script src="../js/bootstrap.min.js"></script>
|
||
|
|
<script src="../js/jquery.stickytableheaders.min.js"></script>
|
||
|
|
<script src="../js/readtheorg.js"></script>
|
||
|
|
<script type="text/javascript">
|
||
|
|
/*
|
||
|
|
@licstart The following is the entire license notice for the
|
||
|
|
JavaScript code in this tag.
|
||
|
|
|
||
|
|
Copyright (C) 2012-2019 Free Software Foundation, Inc.
|
||
|
|
|
||
|
|
The JavaScript code in this tag is free software: you can
|
||
|
|
redistribute it and/or modify it under the terms of the GNU
|
||
|
|
General Public License (GNU GPL) as published by the Free Software
|
||
|
|
Foundation, either version 3 of the License, or (at your option)
|
||
|
|
any later version. The code is distributed WITHOUT ANY WARRANTY;
|
||
|
|
without even the implied warranty of MERCHANTABILITY or FITNESS
|
||
|
|
FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
|
||
|
|
|
||
|
|
As additional permission under GNU GPL version 3 section 7, you
|
||
|
|
may distribute non-source (e.g., minimized or compacted) forms of
|
||
|
|
that code without the copy of the GNU GPL normally required by
|
||
|
|
section 4, provided you include this license notice and a URL
|
||
|
|
through which recipients can access the Corresponding Source.
|
||
|
|
|
||
|
|
|
||
|
|
@licend The above is the entire license notice
|
||
|
|
for the JavaScript code in this tag.
|
||
|
|
*/
|
||
|
|
<!--/*--><![CDATA[/*><!--*/
|
||
|
|
function CodeHighlightOn(elem, id)
|
||
|
|
{
|
||
|
|
var target = document.getElementById(id);
|
||
|
|
if(null != target) {
|
||
|
|
elem.cacheClassElem = elem.className;
|
||
|
|
elem.cacheClassTarget = target.className;
|
||
|
|
target.className = "code-highlighted";
|
||
|
|
elem.className = "code-highlighted";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
function CodeHighlightOff(elem, id)
|
||
|
|
{
|
||
|
|
var target = document.getElementById(id);
|
||
|
|
if(elem.cacheClassElem)
|
||
|
|
elem.className = elem.cacheClassElem;
|
||
|
|
if(elem.cacheClassTarget)
|
||
|
|
target.className = elem.cacheClassTarget;
|
||
|
|
}
|
||
|
|
/*]]>*///-->
|
||
|
|
</script>
|
||
|
|
<script type="text/x-mathjax-config">
|
||
|
|
MathJax.Hub.Config({
|
||
|
|
displayAlign: "center",
|
||
|
|
displayIndent: "0em",
|
||
|
|
|
||
|
|
"HTML-CSS": { scale: 100,
|
||
|
|
linebreaks: { automatic: "false" },
|
||
|
|
webFont: "TeX"
|
||
|
|
},
|
||
|
|
SVG: {scale: 100,
|
||
|
|
linebreaks: { automatic: "false" },
|
||
|
|
font: "TeX"},
|
||
|
|
NativeMML: {scale: 100},
|
||
|
|
TeX: { equationNumbers: {autoNumber: "AMS"},
|
||
|
|
MultLineWidth: "85%",
|
||
|
|
TagSide: "right",
|
||
|
|
TagIndent: ".8em"
|
||
|
|
}
|
||
|
|
});
|
||
|
|
</script>
|
||
|
|
<script type="text/javascript"
|
||
|
|
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
|
||
|
|
</head>
|
||
|
|
<body>
|
||
|
|
<div id="org-div-home-and-up">
|
||
|
|
<a accesskey="h" href="../index.html"> UP </a>
|
||
|
|
|
|
||
|
|
<a accesskey="H" href="../index.html"> HOME </a>
|
||
|
|
</div><div id="content">
|
||
|
|
<h1 class="title">On the Design of Complementary Filters for Control - Computation with Matlab</h1>
|
||
|
|
<div id="table-of-contents">
|
||
|
|
<h2>Table of Contents</h2>
|
||
|
|
<div id="text-table-of-contents">
|
||
|
|
<ul>
|
||
|
|
<li><a href="#orgc8a0b0b">1. Optimal Sensor Fusion for noise characteristics</a>
|
||
|
|
<ul>
|
||
|
|
<li><a href="#org04ee05b">1.1. Architecture</a></li>
|
||
|
|
<li><a href="#org48007f5">1.2. Noise of the sensors</a></li>
|
||
|
|
<li><a href="#org76a1145">1.3. H-Two Synthesis</a></li>
|
||
|
|
<li><a href="#orga04dac6">1.4. Analysis</a></li>
|
||
|
|
</ul>
|
||
|
|
</li>
|
||
|
|
<li><a href="#org55b10e6">2. Robustness to sensor dynamics uncertainty</a>
|
||
|
|
<ul>
|
||
|
|
<li><a href="#orgdac3786">2.1. Unknown sensor dynamics dynamics</a></li>
|
||
|
|
<li><a href="#org3bbb654">2.2. Design the complementary filters in order to limit the phase and gain uncertainty of the super sensor</a></li>
|
||
|
|
<li><a href="#org90b0e78">2.3. First Basic Example with gain mismatch</a></li>
|
||
|
|
<li><a href="#org8b3719c">2.4. <span class="todo TODO">TODO</span> More Complete example with model uncertainty</a></li>
|
||
|
|
</ul>
|
||
|
|
</li>
|
||
|
|
<li><a href="#org8d0e75b">3. Complementary filters using analytical formula</a>
|
||
|
|
<ul>
|
||
|
|
<li><a href="#org418be39">3.1. Analytical 1st order complementary filters</a></li>
|
||
|
|
<li><a href="#orgce86bef">3.2. Second Order Complementary Filters</a></li>
|
||
|
|
<li><a href="#org3a076cb">3.3. Third Order Complementary Filters</a></li>
|
||
|
|
</ul>
|
||
|
|
</li>
|
||
|
|
<li><a href="#orgb74760b">4. H-Infinity synthesis of complementary filters</a>
|
||
|
|
<ul>
|
||
|
|
<li><a href="#org64a93c7">4.1. Synthesis Architecture</a></li>
|
||
|
|
<li><a href="#orgeb8d3d5">4.2. Weights</a></li>
|
||
|
|
<li><a href="#orgc82fc2f">4.3. H-Infinity Synthesis</a></li>
|
||
|
|
<li><a href="#org49c8dfa">4.4. Obtained Complementary Filters</a></li>
|
||
|
|
</ul>
|
||
|
|
</li>
|
||
|
|
<li><a href="#org1a55138">5. Feedback Control Architecture to generate Complementary Filters</a>
|
||
|
|
<ul>
|
||
|
|
<li><a href="#orga294f6c">5.1. Architecture</a></li>
|
||
|
|
<li><a href="#orgc9b9e6a">5.2. Loop Gain Design</a></li>
|
||
|
|
<li><a href="#org10f79fb">5.3. Complementary Filters Obtained</a></li>
|
||
|
|
</ul>
|
||
|
|
</li>
|
||
|
|
</ul>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
In this document, the design of complementary filters is studied.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
One use of complementary filter is described below:
|
||
|
|
</p>
|
||
|
|
<blockquote>
|
||
|
|
<p>
|
||
|
|
The basic idea of a complementary filter involves taking two or more sensors, filtering out unreliable frequencies for each sensor, and combining the filtered outputs to get a better estimate throughout the entire bandwidth of the system.
|
||
|
|
To achieve this, the sensors included in the filter should complement one another by performing better over specific parts of the system bandwidth.
|
||
|
|
</p>
|
||
|
|
</blockquote>
|
||
|
|
|
||
|
|
<ul class="org-ul">
|
||
|
|
<li>in section <a href="#org796449a">1</a>, the optimal design of the complementary filters in order to obtain the lowest resulting "super sensor" noise is studied</li>
|
||
|
|
</ul>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
When blending two sensors using complementary filters with unknown dynamics, phase lag may be introduced that renders the close-loop system unstable.
|
||
|
|
</p>
|
||
|
|
<ul class="org-ul">
|
||
|
|
<li>in section <a href="#orge343e91">2</a>, the blending robustness to sensor dynamic uncertainty is studied.</li>
|
||
|
|
</ul>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Then, three design methods for generating two complementary filters are proposed:
|
||
|
|
</p>
|
||
|
|
<ul class="org-ul">
|
||
|
|
<li>in section <a href="#org788d7fc">3</a>, analytical formulas are proposed</li>
|
||
|
|
<li>in section <a href="#org03b46b0">4</a>, the \(\mathcal{H}_\infty\) synthesis is used</li>
|
||
|
|
<li>in section <a href="#org0636b4b">5</a>, the classical feedback architecture is used</li>
|
||
|
|
</ul>
|
||
|
|
|
||
|
|
<div id="outline-container-orgc8a0b0b" class="outline-2">
|
||
|
|
<h2 id="orgc8a0b0b"><span class="section-number-2">1</span> Optimal Sensor Fusion for noise characteristics</h2>
|
||
|
|
<div class="outline-text-2" id="text-1">
|
||
|
|
<p>
|
||
|
|
<a id="org796449a"></a>
|
||
|
|
</p>
|
||
|
|
<p>
|
||
|
|
The idea is to combine sensors that works in different frequency range using complementary filters.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Doing so, one "super sensor" is obtained that can have better noise characteristics than the individual sensors over a large frequency range.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise.
|
||
|
|
</p>
|
||
|
|
<div class="note">
|
||
|
|
<p>
|
||
|
|
All the files (data and Matlab scripts) are accessible <a href="data/optimal_comp_filters.zip">here</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org04ee05b" class="outline-3">
|
||
|
|
<h3 id="org04ee05b"><span class="section-number-3">1.1</span> Architecture</h3>
|
||
|
|
<div class="outline-text-3" id="text-1-1">
|
||
|
|
<p>
|
||
|
|
Let's consider the sensor fusion architecture shown on figure <a href="#org66ff4fe">1</a> where two sensors 1 and 2 are measuring the same quantity \(x\) with different noise characteristics determined by \(W_1\) and \(W_2\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
\(n_1\) and \(n_2\) are white noise (constant power spectral density over all frequencies).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org66ff4fe" class="figure">
|
||
|
|
<p><img src="figs/fusion_two_noisy_sensors_with_dyn.png" alt="fusion_two_noisy_sensors_with_dyn.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 1: </span>Fusion of two sensors</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We consider that the two sensor dynamics \(G_1\) and \(G_2\) are ideal (\(G_1 = G_2 = 1\)). We obtain the architecture of figure <a href="#org8a1b0d3">2</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org8a1b0d3" class="figure">
|
||
|
|
<p><img src="figs/fusion_two_noisy_sensors.png" alt="fusion_two_noisy_sensors.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 2: </span>Fusion of two sensors with ideal dynamics</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
\(H_1\) and \(H_2\) are complementary filters (\(H_1 + H_2 = 1\)). The goal is to design \(H_1\) and \(H_2\) such that the effect of the noise sources \(n_1\) and \(n_2\) has the smallest possible effect on the estimation \(\hat{x}\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We have that the Power Spectral Density (PSD) of \(\hat{x}\) is:
|
||
|
|
\[ \Gamma_{\hat{x}} = |H_1 W_1|^2 \Gamma_{n_1} + |H_2 W_2|^2 \Gamma_{n_2} \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
And the goal is the minimize the Root Mean Square (RMS) value of \(\hat{x}\):
|
||
|
|
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Gamma_{\hat{x}}(\omega) d\omega} \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
As \(n_1\) and \(n_2\) are white noise: \(\Gamma_{n_1} = \Gamma_{n_2} = 1\) and we have:
|
||
|
|
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty |H_1 W_1|^2(\omega) + |H_2 W_2|^2(\omega) d\omega} = \left\| \begin{matrix} H_1 W_1 \\ H_2 W_2 \end{matrix} \right\|_2 \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Thus, the goal is to design \(H_1\) and \(H_2\) such that \(H_1 + H_2 = 1\) and such that \(\left\| \begin{matrix} H_1 W_1 \\ H_2 W_2 \end{matrix} \right\|_2\) is minimized.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
For that, we will use the \(\mathcal{H}_2\) Synthesis.
|
||
|
|
</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org48007f5" class="outline-3">
|
||
|
|
<h3 id="org48007f5"><span class="section-number-3">1.2</span> Noise of the sensors</h3>
|
||
|
|
<div class="outline-text-3" id="text-1-2">
|
||
|
|
<p>
|
||
|
|
Let's define the noise characteristics of the two sensors by choosing \(W_1\) and \(W_2\):
|
||
|
|
</p>
|
||
|
|
<ul class="org-ul">
|
||
|
|
<li>Sensor 1 characterized by \(W_1\) has low noise at low frequency (for instance a geophone)</li>
|
||
|
|
<li>Sensor 2 characterized by \(W_2\) has low noise at high frequency (for instance an accelerometer)</li>
|
||
|
|
</ul>
|
||
|
|
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">omegac = <span class="org-highlight-numbers-number">100</span><span class="org-type">*</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span>; G0 = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">5</span>; Ginf = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">4</span>;
|
||
|
|
W1 = <span class="org-rainbow-delimiters-depth-1">(</span>Ginf<span class="org-type">*</span>s<span class="org-type">/</span>omegac <span class="org-type">+</span> G0<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>s<span class="org-type">/</span>omegac <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span> <span class="org-type">+</span> s<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">4000</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
|
||
|
|
omegac = <span class="org-highlight-numbers-number">1</span><span class="org-type">*</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span>; G0 = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>; Ginf = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">8</span>;
|
||
|
|
W2 = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>sqrt<span class="org-rainbow-delimiters-depth-3">(</span>Ginf<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">*</span>s<span class="org-type">/</span>omegac <span class="org-type">+</span> sqrt<span class="org-rainbow-delimiters-depth-3">(</span>G0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>omegac <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span> <span class="org-type">+</span> s<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">4000</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org2400ae4" class="figure">
|
||
|
|
<p><img src="figs/nosie_characteristics_sensors.png" alt="nosie_characteristics_sensors.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 3: </span>Noise Characteristics of the two sensors (<a href="./figs/nosie_characteristics_sensors.png">png</a>, <a href="./figs/nosie_characteristics_sensors.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org76a1145" class="outline-3">
|
||
|
|
<h3 id="org76a1145"><span class="section-number-3">1.3</span> H-Two Synthesis</h3>
|
||
|
|
<div class="outline-text-3" id="text-1-3">
|
||
|
|
<p>
|
||
|
|
We use the generalized plant architecture shown on figure <a href="#org593a2df">4</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org593a2df" class="figure">
|
||
|
|
<p><img src="figs/h_infinity_optimal_comp_filters.png" alt="h_infinity_optimal_comp_filters.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 4: </span>\(\mathcal{H}_2\) Synthesis - Generalized plant used for the optimal generation of complementary filters</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The transfer function from \([n_1, n_2]\) to \(\hat{x}\) is:
|
||
|
|
\[ \begin{bmatrix} W_1 H_1 \\ W_2 (1 - H_1) \end{bmatrix} \]
|
||
|
|
If we define \(H_2 = 1 - H_1\), we obtain:
|
||
|
|
\[ \begin{bmatrix} W_1 H_1 \\ W_2 H_2 \end{bmatrix} \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Thus, if we minimize the \(\mathcal{H}_2\) norm of this transfer function, we minimize the RMS value of \(\hat{x}\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We define the generalized plant \(P\) on matlab as shown on figure <a href="#org593a2df">4</a>.
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">P = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> W2 <span class="org-highlight-numbers-number">1</span>;
|
||
|
|
W1 <span class="org-type">-</span>W2 <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
And we do the \(\mathcal{H}_2\) synthesis using the <code>h2syn</code> command.
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab"><span class="org-rainbow-delimiters-depth-1">[</span>H1, <span class="org-type">~</span>, gamma<span class="org-rainbow-delimiters-depth-1">]</span> = h2syn<span class="org-rainbow-delimiters-depth-1">(</span>P, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
What is minimized is <code>norm([W1*H1,W2*H2], 2)</code>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Finally, we define \(H_2 = 1 - H_1\).
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">H2 = <span class="org-highlight-numbers-number">1</span> <span class="org-type">-</span> H1;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orga04dac6" class="outline-3">
|
||
|
|
<h3 id="orga04dac6"><span class="section-number-3">1.4</span> Analysis</h3>
|
||
|
|
<div class="outline-text-3" id="text-1-4">
|
||
|
|
<p>
|
||
|
|
The complementary filters obtained are shown on figure <a href="#org9196cc2">5</a>. The PSD of the <a href="#orgcec5bf7">6</a>.
|
||
|
|
Finally, the RMS value of \(\hat{x}\) is shown on table <a href="#org18d42aa">1</a>.
|
||
|
|
The optimal sensor fusion has permitted to reduced the RMS value of the estimation error by a factor 8 compare to when using only one sensor.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org9196cc2" class="figure">
|
||
|
|
<p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 5: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis (<a href="./figs/htwo_comp_filters.png">png</a>, <a href="./figs/htwo_comp_filters.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="orgcec5bf7" class="figure">
|
||
|
|
<p><img src="figs/psd_sensors_htwo_synthesis.png" alt="psd_sensors_htwo_synthesis.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 6: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal (<a href="./figs/psd_sensors_htwo_synthesis.png">png</a>, <a href="./figs/psd_sensors_htwo_synthesis.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<table id="org18d42aa" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||
|
|
<caption class="t-above"><span class="table-number">Table 1:</span> RMS value of the estimation error when using the sensor individually and when using the two sensor merged using the optimal complementary filters</caption>
|
||
|
|
|
||
|
|
<colgroup>
|
||
|
|
<col class="org-left" />
|
||
|
|
|
||
|
|
<col class="org-right" />
|
||
|
|
</colgroup>
|
||
|
|
<thead>
|
||
|
|
<tr>
|
||
|
|
<th scope="col" class="org-left"> </th>
|
||
|
|
<th scope="col" class="org-right">rms value</th>
|
||
|
|
</tr>
|
||
|
|
</thead>
|
||
|
|
<tbody>
|
||
|
|
<tr>
|
||
|
|
<td class="org-left">Sensor 1</td>
|
||
|
|
<td class="org-right">1.1e-02</td>
|
||
|
|
</tr>
|
||
|
|
|
||
|
|
<tr>
|
||
|
|
<td class="org-left">Sensor 2</td>
|
||
|
|
<td class="org-right">1.3e-03</td>
|
||
|
|
</tr>
|
||
|
|
|
||
|
|
<tr>
|
||
|
|
<td class="org-left">Optimal Sensor Fusion</td>
|
||
|
|
<td class="org-right">1.5e-04</td>
|
||
|
|
</tr>
|
||
|
|
</tbody>
|
||
|
|
</table>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
<div id="outline-container-org55b10e6" class="outline-2">
|
||
|
|
<h2 id="org55b10e6"><span class="section-number-2">2</span> Robustness to sensor dynamics uncertainty</h2>
|
||
|
|
<div class="outline-text-2" id="text-2">
|
||
|
|
<p>
|
||
|
|
<a id="orge343e91"></a>
|
||
|
|
</p>
|
||
|
|
<p>
|
||
|
|
Let's first consider ideal sensors where \(G_1 = 1\) and \(G_2 = 1\) (figure <a href="#org524433d">7</a>).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org524433d" class="figure">
|
||
|
|
<p><img src="figs/fusion_two_noisy_sensors_with_dyn_bis.png" alt="fusion_two_noisy_sensors_with_dyn_bis.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 7: </span>Fusion of two sensors</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We then have:
|
||
|
|
</p>
|
||
|
|
\begin{align*}
|
||
|
|
\hat{x} &= (x + n_1) H_1 + (x + n_2) H_2 \\
|
||
|
|
&= x + n_1 H_1 + n_2 H_2
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
<p>
|
||
|
|
So the estimation error is
|
||
|
|
\[ \delta_x = \hat{x} - x = n_1 H_1 + n_2 H_2 \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
And we see that the complementary filters are only shaping the noise and that they do not impact the transfer function from \(x\) to \(\hat{x}\) that is in the feedback path.
|
||
|
|
</p>
|
||
|
|
<div class="note">
|
||
|
|
<p>
|
||
|
|
All the files (data and Matlab scripts) are accessible <a href="data/comp_filter_robustness.zip">here</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orgdac3786" class="outline-3">
|
||
|
|
<h3 id="orgdac3786"><span class="section-number-3">2.1</span> Unknown sensor dynamics dynamics</h3>
|
||
|
|
<div class="outline-text-3" id="text-2-1">
|
||
|
|
<p>
|
||
|
|
In practical systems, the sensor dynamics has always some level of uncertainty.
|
||
|
|
Let's represent that with multiplicative input uncertainty as shown on figure <a href="#org00c6572">8</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org00c6572" class="figure">
|
||
|
|
<p><img src="figs/fusion_gain_mismatch.png" alt="fusion_gain_mismatch.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 8: </span>Fusion of two sensors with input multiplicative uncertainty</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We have:
|
||
|
|
</p>
|
||
|
|
\begin{align*}
|
||
|
|
\frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\
|
||
|
|
&= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2
|
||
|
|
\end{align*}
|
||
|
|
<p>
|
||
|
|
With \(\Delta_i\) is any transfer function satisfying \(\| \Delta_i \|_\infty < 1\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from \(x\) to \(\hat{x}\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We want that the super sensor transfer function has a gain of 1 and no phase variation over all the frequencies:
|
||
|
|
\[ \frac{\hat{x}}{x} \approx 1 \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Thus, we want that
|
||
|
|
</p>
|
||
|
|
\begin{align*}
|
||
|
|
& |W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2| < \epsilon \quad \forall \omega, \forall \Delta_i, \|\Delta_i\|_\infty < 1 \\
|
||
|
|
\Longleftrightarrow & |W_1 H_1| + |W_2 H_2| < \epsilon \quad \forall \omega
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Which is approximately the same as requiring
|
||
|
|
\[ \left\| \begin{matrix} W_1 H_1 \\ W_2 H_2 \end{matrix} \right\|_\infty < \epsilon \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<p>
|
||
|
|
<b>How small should we choose \(\epsilon\)?</b>
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(\epsilon\) (figure <a href="#org6e574cc">9</a>).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We then have that the angle introduced by the super sensor is bounded by \(\arcsin(\epsilon)\):
|
||
|
|
\[ \angle \frac{\hat{x}}{x} \le \arcsin (\epsilon) \quad \forall \omega \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org6e574cc" class="figure">
|
||
|
|
<p><img src="figs/uncertainty_gain_phase_variation.png" alt="uncertainty_gain_phase_variation.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 9: </span>Maximum phase variation</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Thus, we choose should choose \(\epsilon\) so that the maximum phase uncertainty introduced by the sensors is of an acceptable value.
|
||
|
|
</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org3bbb654" class="outline-3">
|
||
|
|
<h3 id="org3bbb654"><span class="section-number-3">2.2</span> Design the complementary filters in order to limit the phase and gain uncertainty of the super sensor</h3>
|
||
|
|
<div class="outline-text-3" id="text-2-2">
|
||
|
|
<p>
|
||
|
|
Let's say the two sensors dynamics \(H_1\) and \(H_2\) have been identified with the associated uncertainty weights \(W_1\) and \(W_2\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
If we want to have a maximum phase introduced by the sensors of 20 degrees, we have to design \(H_1\) and \(H_2\) such that:
|
||
|
|
</p>
|
||
|
|
\begin{align*}
|
||
|
|
& arcsin(|H_1 W_1| + |H_2 W_2|) < 20 \text{ deg} \\
|
||
|
|
\Longleftrightarrow & |H_1 W_1| + |H_2 W_2| < 0.34
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We can do that with the \(\mathcal{H}_\infty\) synthesis by setting upper bounds on the complementary filters using weights that corresponds to the sensor dynamics uncertainty.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
For simplicity, let's suppose \(W_1(s) = W_2(s) = 0.1\) (\(10\%\) uncertainty in the sensor gain).
|
||
|
|
\[ |H_1 W_1| + |H_2 W_2| < 3.4 \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Thus, by limiting the norm of the complementary filters, we can limit the maximum unwanted phase introduced by the uncertainty on the sensors dynamics.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
This is of primary importance in order to ensure the stability of the feedback loop using the super sensor signal.
|
||
|
|
</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org90b0e78" class="outline-3">
|
||
|
|
<h3 id="org90b0e78"><span class="section-number-3">2.3</span> First Basic Example with gain mismatch</h3>
|
||
|
|
<div class="outline-text-3" id="text-2-3">
|
||
|
|
<p>
|
||
|
|
Let's consider two ideal sensors except one sensor has not an expected gain of one but a gain of \(0.6\).
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">G1 = <span class="org-highlight-numbers-number">1</span>;
|
||
|
|
G2 = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">6</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Let's design two complementary filters as shown on figure <a href="#orga15f970">10</a>.
|
||
|
|
The complementary filters shown in blue does not present a bump as the red ones but provides less sensor separation at high and low frequencies.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="orga15f970" class="figure">
|
||
|
|
<p><img src="figs/comp_filters_robustness_test.png" alt="comp_filters_robustness_test.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 10: </span>The two complementary filters designed for the robustness test (<a href="./figs/comp_filters_robustness_test.png">png</a>, <a href="./figs/comp_filters_robustness_test.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We then compute the bode plot of the super sensor transfer function \(H_1*G_1 + H_2*G_2\) for both complementary filters pair (figure <a href="#orgac681c0">11</a>).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We see that the blue complementary filters with a lower maximum norm permits to limit the phase lag introduced by the gain mismatch.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="orgac681c0" class="figure">
|
||
|
|
<p><img src="figs/tf_super_sensor_comp.png" alt="tf_super_sensor_comp.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 11: </span>Comparison of the obtained super sensor transfer functions (<a href="./figs/tf_super_sensor_comp.png">png</a>, <a href="./figs/tf_super_sensor_comp.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org8b3719c" class="outline-3">
|
||
|
|
<h3 id="org8b3719c"><span class="section-number-3">2.4</span> <span class="todo TODO">TODO</span> More Complete example with model uncertainty</h3>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
<div id="outline-container-org8d0e75b" class="outline-2">
|
||
|
|
<h2 id="org8d0e75b"><span class="section-number-2">3</span> Complementary filters using analytical formula</h2>
|
||
|
|
<div class="outline-text-2" id="text-3">
|
||
|
|
<p>
|
||
|
|
<a id="org788d7fc"></a>
|
||
|
|
</p>
|
||
|
|
<div class="note">
|
||
|
|
<p>
|
||
|
|
All the files (data and Matlab scripts) are accessible <a href="data/comp_filters_analytical.zip">here</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org418be39" class="outline-3">
|
||
|
|
<h3 id="org418be39"><span class="section-number-3">3.1</span> Analytical 1st order complementary filters</h3>
|
||
|
|
<div class="outline-text-3" id="text-3-1">
|
||
|
|
<p>
|
||
|
|
First order complementary filters are defined with following equations:
|
||
|
|
</p>
|
||
|
|
\begin{align}
|
||
|
|
H_L(s) = \frac{1}{1 + \frac{s}{\omega_0}}\\
|
||
|
|
H_H(s) = \frac{\frac{s}{\omega_0}}{1 + \frac{s}{\omega_0}}
|
||
|
|
\end{align}
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Their bode plot is shown figure <a href="#org216cb68">12</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">w0 = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span>; <span class="org-comment">% [rad/s]</span>
|
||
|
|
|
||
|
|
Hh1 = <span class="org-rainbow-delimiters-depth-1">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
Hl1 = <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org216cb68" class="figure">
|
||
|
|
<p><img src="figs/comp_filter_1st_order.png" alt="comp_filter_1st_order.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 12: </span>Bode plot of first order complementary filter (<a href="./figs/comp_filter_1st_order.png">png</a>, <a href="./figs/comp_filter_1st_order.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orgce86bef" class="outline-3">
|
||
|
|
<h3 id="orgce86bef"><span class="section-number-3">3.2</span> Second Order Complementary Filters</h3>
|
||
|
|
<div class="outline-text-3" id="text-3-2">
|
||
|
|
<p>
|
||
|
|
We here use analytical formula for the complementary filters \(H_L\) and \(H_H\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The first two formulas that are used to generate complementary filters are:
|
||
|
|
</p>
|
||
|
|
\begin{align*}
|
||
|
|
H_L(s) &= \frac{(1+\alpha) (\frac{s}{\omega_0})+1}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}\\
|
||
|
|
H_H(s) &= \frac{(\frac{s}{\omega_0})^2 \left((\frac{s}{\omega_0})+1+\alpha\right)}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}
|
||
|
|
\end{align*}
|
||
|
|
<p>
|
||
|
|
where:
|
||
|
|
</p>
|
||
|
|
<ul class="org-ul">
|
||
|
|
<li>\(\omega_0\) is the blending frequency in rad/s.</li>
|
||
|
|
<li>\(\alpha\) is used to change the shape of the filters:
|
||
|
|
<ul class="org-ul">
|
||
|
|
<li>Small values for \(\alpha\) will produce high magnitude of the filters \(|H_L(j\omega)|\) and \(|H_H(j\omega)|\) near \(\omega_0\) but smaller value for \(|H_L(j\omega)|\) above \(\approx 1.5 \omega_0\) and for \(|H_H(j\omega)|\) below \(\approx 0.7 \omega_0\)</li>
|
||
|
|
<li>A large \(\alpha\) will do the opposite</li>
|
||
|
|
</ul></li>
|
||
|
|
</ul>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
This is illustrated on figure <a href="#org8c54c64">13</a>.
|
||
|
|
The slope of those filters at high and low frequencies is \(-2\) and \(2\) respectively for \(H_L\) and \(H_H\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org8c54c64" class="figure">
|
||
|
|
<p><img src="figs/comp_filters_param_alpha.png" alt="comp_filters_param_alpha.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 13: </span>Effect of the parameter \(\alpha\) on the shape of the generated second order complementary filters (<a href="./figs/comp_filters_param_alpha.png">png</a>, <a href="./figs/comp_filters_param_alpha.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We now study the maximum norm of the filters function of the parameter \(\alpha\). As we saw that the maximum norm of the filters is important for the robust merging of filters.
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
||
|
|
plot<span class="org-rainbow-delimiters-depth-1">(</span>alphas, infnorms<span class="org-rainbow-delimiters-depth-1">)</span>
|
||
|
|
<span class="org-type">set</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">gca</span>, <span class="org-string">'xscale', 'log'</span><span class="org-string"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-string">; set</span><span class="org-string"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-string">gca, 'yscale', 'log'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
xlabel<span class="org-rainbow-delimiters-depth-1">(</span>'$<span class="org-type">\</span>alpha$'<span class="org-rainbow-delimiters-depth-1">)</span>; ylabel<span class="org-rainbow-delimiters-depth-1">(</span>'$<span class="org-type">\|</span>H_1<span class="org-type">\|</span>_<span class="org-type">\</span>infty$'<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="orgc29bd71" class="figure">
|
||
|
|
<p><img src="figs/param_alpha_hinf_norm.png" alt="param_alpha_hinf_norm.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 14: </span>Evolution of the H-Infinity norm of the complementary filters with the parameter \(\alpha\) (<a href="./figs/param_alpha_hinf_norm.png">png</a>, <a href="./figs/param_alpha_hinf_norm.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org3a076cb" class="outline-3">
|
||
|
|
<h3 id="org3a076cb"><span class="section-number-3">3.3</span> Third Order Complementary Filters</h3>
|
||
|
|
<div class="outline-text-3" id="text-3-3">
|
||
|
|
<p>
|
||
|
|
The following formula gives complementary filters with slopes of \(-3\) and \(3\):
|
||
|
|
</p>
|
||
|
|
\begin{align*}
|
||
|
|
H_L(s) &= \frac{\left(1+(\alpha+1)(\beta+1)\right) (\frac{s}{\omega_0})^2 + (1+\alpha+\beta)(\frac{s}{\omega_0}) + 1}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}\\
|
||
|
|
H_H(s) &= \frac{(\frac{s}{\omega_0})^3 \left( (\frac{s}{\omega_0})^2 + (1+\alpha+\beta) (\frac{s}{\omega_0}) + (1+(\alpha+1)(\beta+1)) \right)}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The parameters are:
|
||
|
|
</p>
|
||
|
|
<ul class="org-ul">
|
||
|
|
<li>\(\omega_0\) is the blending frequency in rad/s</li>
|
||
|
|
<li>\(\alpha\) and \(\beta\) that are used to change the shape of the filters similarly to the parameter \(\alpha\) for the second order complementary filters</li>
|
||
|
|
</ul>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The filters are defined below and the result is shown on figure <a href="#org260ae95">15</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">alpha = <span class="org-highlight-numbers-number">1</span>;
|
||
|
|
beta = <span class="org-highlight-numbers-number">10</span>;
|
||
|
|
w0 = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span><span class="org-highlight-numbers-number">14</span>;
|
||
|
|
|
||
|
|
Hh3_ana = <span class="org-rainbow-delimiters-depth-1">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">3</span> <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span> <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">+</span>alpha<span class="org-type">+</span>beta<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">+</span><span class="org-rainbow-delimiters-depth-3">(</span>alpha<span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-3">(</span>beta<span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0 <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">+</span>alpha<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">+</span>beta<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
Hl3_ana = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">+</span><span class="org-rainbow-delimiters-depth-3">(</span>alpha<span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-3">(</span>beta<span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span> <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">+</span>alpha<span class="org-type">+</span>beta<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>s<span class="org-type">/</span>w0 <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">+</span>alpha<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">+</span>beta<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-3">(</span>s<span class="org-type">/</span>w0<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org260ae95" class="figure">
|
||
|
|
<p><img src="figs/complementary_filters_third_order.png" alt="complementary_filters_third_order.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 15: </span>Third order complementary filters using the analytical formula (<a href="./figs/complementary_filters_third_order.png">png</a>, <a href="./figs/complementary_filters_third_order.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orgb74760b" class="outline-2">
|
||
|
|
<h2 id="orgb74760b"><span class="section-number-2">4</span> H-Infinity synthesis of complementary filters</h2>
|
||
|
|
<div class="outline-text-2" id="text-4">
|
||
|
|
<p>
|
||
|
|
<a id="org03b46b0"></a>
|
||
|
|
</p>
|
||
|
|
<div class="note">
|
||
|
|
<p>
|
||
|
|
All the files (data and Matlab scripts) are accessible <a href="data/h_inf_synthesis_complementary_filters.zip">here</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org64a93c7" class="outline-3">
|
||
|
|
<h3 id="org64a93c7"><span class="section-number-3">4.1</span> Synthesis Architecture</h3>
|
||
|
|
<div class="outline-text-3" id="text-4-1">
|
||
|
|
<p>
|
||
|
|
We here synthesize the complementary filters using the \(\mathcal{H}_\infty\) synthesis.
|
||
|
|
The goal is to specify upper bounds on the norms of \(H_L\) and \(H_H\) while ensuring their complementary property (\(H_L + H_H = 1\)).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
In order to do so, we use the generalized plant shown on figure <a href="#org9c465a4">16</a> where \(w_L\) and \(w_H\) weighting transfer functions that will be used to shape \(H_L\) and \(H_H\) respectively.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org9c465a4" class="figure">
|
||
|
|
<p><img src="figs/sf_hinf_filters_plant_b.png" alt="sf_hinf_filters_plant_b.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 16: </span>Generalized plant used for the \(\mathcal{H}_\infty\) synthesis of the complementary filters</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The \(\mathcal{H}_\infty\) synthesis applied on this generalized plant will give a transfer function \(H_L\) (figure <a href="#org7cda9f1">17</a>) such that the \(\mathcal{H}_\infty\) norm of the transfer function from \(w\) to \([z_H,\ z_L]\) is less than one:
|
||
|
|
\[ \left\| \begin{array}{c} H_L w_L \\ (1 - H_L) w_H \end{array} \right\|_\infty < 1 \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Thus, if the above condition is verified, we can define \(H_H = 1 - H_L\) and we have that:
|
||
|
|
\[ \left\| \begin{array}{c} H_L w_L \\ H_H w_H \end{array} \right\|_\infty < 1 \]
|
||
|
|
Which is almost (with an maximum error of \(\sqrt{2}\)) equivalent to:
|
||
|
|
</p>
|
||
|
|
\begin{align*}
|
||
|
|
|H_L| &< \frac{1}{|w_L|}, \quad \forall \omega \\
|
||
|
|
|H_H| &< \frac{1}{|w_H|}, \quad \forall \omega
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We then see that \(w_L\) and \(w_H\) can be used to shape both \(H_L\) and \(H_H\) while ensuring (by definition of \(H_H = 1 - H_L\)) their complementary property.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org7cda9f1" class="figure">
|
||
|
|
<p><img src="figs/sf_hinf_filters_b.png" alt="sf_hinf_filters_b.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 17: </span>\(\mathcal{H}_\infty\) synthesis of the complementary filters</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orgeb8d3d5" class="outline-3">
|
||
|
|
<h3 id="orgeb8d3d5"><span class="section-number-3">4.2</span> Weights</h3>
|
||
|
|
<div class="outline-text-3" id="text-4-2">
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">omegab = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span><span class="org-highlight-numbers-number">9</span>;
|
||
|
|
wH = <span class="org-rainbow-delimiters-depth-1">(</span>omegab<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>s <span class="org-type">+</span> omegab<span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span>;
|
||
|
|
omegab = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span><span class="org-highlight-numbers-number">28</span>;
|
||
|
|
wL = <span class="org-rainbow-delimiters-depth-1">(</span>s <span class="org-type">+</span> omegab<span class="org-type">/</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">4</span>.<span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">^</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">3</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>s<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">^</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">+</span> omegab<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">3</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org72233b9" class="figure">
|
||
|
|
<p><img src="figs/weights_wl_wh.png" alt="weights_wl_wh.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 18: </span>Weights on the complementary filters \(w_L\) and \(w_H\) and the associated performance weights (<a href="./figs/weights_wl_wh.png">png</a>, <a href="./figs/weights_wl_wh.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orgc82fc2f" class="outline-3">
|
||
|
|
<h3 id="orgc82fc2f"><span class="section-number-3">4.3</span> H-Infinity Synthesis</h3>
|
||
|
|
<div class="outline-text-3" id="text-4-3">
|
||
|
|
<p>
|
||
|
|
We define the generalized plant \(P\) on matlab.
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">P = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> wL;
|
||
|
|
wH <span class="org-type">-</span>wH;
|
||
|
|
<span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
And we do the \(\mathcal{H}_\infty\) synthesis using the <code>hinfsyn</code> command.
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab"><span class="org-rainbow-delimiters-depth-1">[</span>Hl_hinf, <span class="org-type">~</span>, gamma, <span class="org-type">~</span><span class="org-rainbow-delimiters-depth-1">]</span> = hinfsyn<span class="org-rainbow-delimiters-depth-1">(</span>P, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>,'TOLGAM', <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">001</span>, 'METHOD', 'ric', 'DISPLAY', 'on'<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<pre class="example">
|
||
|
|
[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||
|
|
Test bounds: 0.0000 < gamma <= 1.7285
|
||
|
|
|
||
|
|
gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f
|
||
|
|
1.729 4.1e+01 8.4e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.864 3.9e+01 -5.8e-02# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
1.296 4.0e+01 8.4e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
1.080 4.0e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.972 3.9e+01 -4.2e-01# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
1.026 4.0e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.999 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.986 3.9e+01 -1.2e+00# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
0.993 3.9e+01 -8.2e+00# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
0.996 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.994 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.993 3.9e+01 -3.2e+01# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
|
||
|
|
Gamma value achieved: 0.9942
|
||
|
|
</pre>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We then define the high pass filter \(H_H = 1 - H_L\). The bode plot of both \(H_L\) and \(H_H\) is shown on figure <a href="#org653f844">19</a>.
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">Hh_hinf = <span class="org-highlight-numbers-number">1</span> <span class="org-type">-</span> Hl_hinf;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org49c8dfa" class="outline-3">
|
||
|
|
<h3 id="org49c8dfa"><span class="section-number-3">4.4</span> Obtained Complementary Filters</h3>
|
||
|
|
<div class="outline-text-3" id="text-4-4">
|
||
|
|
<p>
|
||
|
|
The obtained complementary filters are shown on figure <a href="#org653f844">19</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org653f844" class="figure">
|
||
|
|
<p><img src="figs/hinf_filters_results.png" alt="hinf_filters_results.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 19: </span>Obtained complementary filters using \(\mathcal{H}_\infty\) synthesis (<a href="./figs/hinf_filters_results.png">png</a>, <a href="./figs/hinf_filters_results.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org1a55138" class="outline-2">
|
||
|
|
<h2 id="org1a55138"><span class="section-number-2">5</span> Feedback Control Architecture to generate Complementary Filters</h2>
|
||
|
|
<div class="outline-text-2" id="text-5">
|
||
|
|
<p>
|
||
|
|
<a id="org0636b4b"></a>
|
||
|
|
</p>
|
||
|
|
<p>
|
||
|
|
The idea is here to use the fact that in a classical feedback architecture, \(S + T = 1\), in order to design complementary filters.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Thus, all the tools that has been developed for classical feedback control can be used for complementary filter design.
|
||
|
|
</p>
|
||
|
|
<div class="note">
|
||
|
|
<p>
|
||
|
|
All the files (data and Matlab scripts) are accessible <a href="data/feedback_generate_comp_filters.zip">here</a>.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orga294f6c" class="outline-3">
|
||
|
|
<h3 id="orga294f6c"><span class="section-number-3">5.1</span> Architecture</h3>
|
||
|
|
<div class="outline-text-3" id="text-5-1">
|
||
|
|
|
||
|
|
<div id="orga2c4a58" class="figure">
|
||
|
|
<p><img src="figs/complementary_filters_feedback_architecture.png" alt="complementary_filters_feedback_architecture.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 20: </span>Architecture used to generate the complementary filters</p>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
We have:
|
||
|
|
\[ y = \underbrace{\frac{L}{L + 1}}_{H_L} y_1 + \underbrace{\frac{1}{L + 1}}_{H_H} y_2 \]
|
||
|
|
with \(H_L + H_H = 1\).
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
The only thing to design is \(L\) such that the complementary filters are stable with the wanted shape.
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
A simple choice is:
|
||
|
|
\[ L = \left(\frac{\omega_c}{s}\right)^2 \frac{\frac{s}{\omega_c / \alpha} + 1}{\frac{s}{\omega_c} + \alpha} \]
|
||
|
|
</p>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
Which contains two integrator and a lead. \(\omega_c\) is used to tune the crossover frequency and \(\alpha\) the trade-off "bump" around blending frequency and filtering away from blending frequency.
|
||
|
|
</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-orgc9b9e6a" class="outline-3">
|
||
|
|
<h3 id="orgc9b9e6a"><span class="section-number-3">5.2</span> Loop Gain Design</h3>
|
||
|
|
<div class="outline-text-3" id="text-5-2">
|
||
|
|
<p>
|
||
|
|
Let's first define the loop gain \(L\).
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">wc = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span><span class="org-highlight-numbers-number">1</span>;
|
||
|
|
alpha = <span class="org-highlight-numbers-number">2</span>;
|
||
|
|
|
||
|
|
L = <span class="org-rainbow-delimiters-depth-1">(</span>wc<span class="org-type">/</span>s<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span> <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span>s<span class="org-type">/</span><span class="org-rainbow-delimiters-depth-2">(</span>wc<span class="org-type">/</span>alpha<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>s<span class="org-type">/</span>wc <span class="org-type">+</span> alpha<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org5d40fdf" class="figure">
|
||
|
|
<p><img src="figs/loop_gain_bode_plot.png" alt="loop_gain_bode_plot.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 21: </span>Bode plot of the loop gain \(L\) (<a href="./figs/loop_gain_bode_plot.png">png</a>, <a href="./figs/loop_gain_bode_plot.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<div id="outline-container-org10f79fb" class="outline-3">
|
||
|
|
<h3 id="org10f79fb"><span class="section-number-3">5.3</span> Complementary Filters Obtained</h3>
|
||
|
|
<div class="outline-text-3" id="text-5-3">
|
||
|
|
<p>
|
||
|
|
We then compute the resulting low pass and high pass filters.
|
||
|
|
</p>
|
||
|
|
<div class="org-src-container">
|
||
|
|
<pre class="src src-matlab">Hl = L<span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>L <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
Hh = <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>L <span class="org-type">+</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||
|
|
</pre>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
|
||
|
|
<div id="org6abcb3c" class="figure">
|
||
|
|
<p><img src="figs/low_pass_high_pass_filters.png" alt="low_pass_high_pass_filters.png" />
|
||
|
|
</p>
|
||
|
|
<p><span class="figure-number">Figure 22: </span>Low pass and High pass filters \(H_L\) and \(H_H\) for different values of \(\alpha\) (<a href="./figs/low_pass_high_pass_filters.png">png</a>, <a href="./figs/low_pass_high_pass_filters.pdf">pdf</a>)</p>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
</div>
|
||
|
|
|
||
|
|
<p>
|
||
|
|
|
||
|
|
<a href="ref.bib">ref.bib</a>
|
||
|
|
</p>
|
||
|
|
</div>
|
||
|
|
<div id="postamble" class="status">
|
||
|
|
<p class="author">Author: Thomas Dehaeze</p>
|
||
|
|
<p class="date">Created: 2019-08-14 mer. 12:08</p>
|
||
|
|
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
|
||
|
|
</div>
|
||
|
|
</body>
|
||
|
|
</html>
|