Cosi119a

* **Viewing: [Kalman Filters](https://www.youtube.com/embed/CaCcOwJPytQ)**

*Reminder: Readings are your responsibility. You will be expected to come to class prepared, having read the material, and ready to participate in the discussion*

<slide_break></slide_break>

### Projects
* Tip from an article
  * For outdoor localizatoin
  * QR Code (not feducial) with the true GPS location encoded in the QR code
* <a href="/content/topics/homeworks/hw-active-119/220_hw_119_proj_milestone/">Project Major Milestone</a>

<slide_break></slide_break>

<h2 id="kalman-filter">Kalman Filter?</h2>

<ul>
  <li>Kalman “Filter” is a mathematical algorithm</li>
  <li>It estimates the true value of an uncertain measurement</li>
  <li>Kalman filters are ideal for systems which are continuously changing</li>
  <li>Given noisy inputs, for example, GPS, Odom and/or IMU</li>
  <li>Calculate, for example, the actual speed of a robot, or the true location of a robot</li>
  <li>Makes an “educated guess” about what the system is going to do next.</li>
</ul>

<slide_break></slide_break>

<h3 id="what-question-does-it-try-to-answer">What question does it try to answer?</h3>

<ul>
  <li>Given uncertain sensor data
    <ol>
      <li>And uncertain state information</li>
      <li>And uncertain control inputs</li>
      <li>And uncertain measurements</li>
      <li>How do you get the best possible estimate of the state?</li>
    </ol>
  </li>
  <li>Well supported by math and statistical analysis
    <ul>
      <li>One key insight two inaccurate readings can be combined into an estimate that is <strong>more</strong> (not less) accurate than either one alone.</li>
      <li>Assuming errors are truly independent</li>
    </ul>
  </li>
</ul>

<slide_break></slide_break>

<h3 id="learning-about-kalman-filters">Learning about Kalman Filters</h3>

<ul>
  <li>There is a lot written about it</li>
  <li>Because it is difficult and important</li>
  <li>The explanations are not consistent and don’t use consistent nomenclature</li>
</ul>

<slide_break></slide_break>

<h3 id="state-estimation">State Estimation</h3>

<ul>
  <li>Kalman filter is an iterative model</li>
  <li>Each loop tries to make the ESTIMATE of the state more accurate</li>
  <li>By processing data from SENSORS (which are statistically characterized)</li>
  <li>And considering known control inputs (which are statistically characterized)</li>
  <li>And unknown perturbations (which are statistically characterized)</li>
  <li>Producing an updated estimate (which is statistically characterized)</li>
  <li>The loops happen at intervals that correspond to the rates that sensors are read</li>
</ul>

<slide_break></slide_break>

<h3 id="scenario">Scenario</h3>

<ul>
  <li>An off-road robot moving around a field</li>
  <li>Using GPS for localization - which has a known inaccuracy on the order of 5-10 meters</li>
  <li>The robot can be asked to move forward or turn at a specified speed or be stationary</li>
  <li>State: position and velocity</li>
  <li>Control: motion commands</li>
  <li>Sensors: GPS … odometry?</li>
</ul>

<slide_break></slide_break>

<h3 id="kalman-estimate">Kalman estimate</h3>

<slide_break></slide_break>

<ul>
  <li>Sensors are everywhere in robotics</li>
  <li>Sensors are known to be inaccurate</li>
  <li>Kalman filters are a family of algorithms *that give us</li>
  <li>More accurate estimates by combining readings with known error rates</li>
  <li>They are efficient and they work</li>
</ul>

<slide_break></slide_break>

<h3 id="system">System</h3>

<ul>
  <li>Kalman filter looks at a “system” which has a “state”</li>
  <li>The system state may change over time
    <ol>
      <li>Based on it’s own internal operation</li>
      <li>Based on “control” input from the outside</li>
      <li>Noise or pertrubation</li>
    </ol>
  </li>
  <li>We gain information about the “system”</li>
  <li>By sensing data which allow us to infer system changes</li>
</ul>

<slide_break></slide_break>

<h3 id="kalman-filter-1">Kalman Filter</h3>

<slide_break></slide_break>

<h3 id="state-model">State model</h3>

<ul>
  <li>Based on what is known about the system</li>
  <li>We can make a first guess of how the old state would change each step</li>
  <li>In other words, if I am at x=0 and I am moving at 1cm / second</li>
  <li>Then 1 second later, I am at x=0.01</li>
  <li>“all things being equal”</li>
  <li>(Note the conceptual similarity to AMCL)</li>
</ul>

<slide_break></slide_break>

<h3 id="state-x">State (X)</h3>

<ul>
  <li>a vector (set) of numbers representing the state you are trying to measure (e.g. location)</li>
  <li>Vector/Matrix representation is for ease of calculation</li>
  <li>But it can also make things look more complicated (for some people)</li>
  <li>Usually denoted by X, containing numbers, here are a series of examples
    <ol>
      <li>current x or y position</li>
      <li>current speed in the x or y direction</li>
      <li>current distance from obstacle</li>
      <li>current altitude from the surface</li>
    </ol>
  </li>
</ul>

<slide_break></slide_break>

<h3 id="covariance-matrix-p">Covariance Matrix (P)</h3>

<ul>
  <li>A way to express the confidence in each component of the state</li>
  <li>And the known correlation between them (Σ)</li>
</ul>

<slide_break></slide_break>

<h3 id="control-model">Control Model</h3>

<ul>
  <li>Look beyond the internal of the system, what is telling it to change?</li>
  <li>Control input:
    <ul>
      <li>Driver steps on break or accelerator</li>
      <li>Gravity is pulling the ball downhill</li>
      <li>cmd_vel is telling the robot to stop</li>
    </ul>
  </li>
  <li>The control model determines the next state given the current state and the control input</li>
  <li>Note: The model often has a random component called process noise.</li>
</ul>

<slide_break></slide_break>

<h3 id="sensor-model">Sensor Model</h3>

<ul>
  <li>Given a measurement from the outside world</li>
  <li>Important: what is the reliability of the sensor (from spec or empirical measurement)</li>
  <li>How does the state model change</li>
</ul>

<slide_break></slide_break>

<h3 id="d-scenario-bathroom-scale">1D Scenario: bathroom scale</h3>

<ul>
  <li>Using a scale with a known error rate</li>
  <li>Sensor generates 10 readings per second</li>
  <li>You know that the weight won’t change (ground truth)</li>
  <li>What do you use? Average? Moving Average? Or?</li>
  <li>What if you have thousands of measurements (10 per second, like a bathroom scale)</li>
  <li>Models:
    <ol>
      <li>State: unchanging (objects weight is  not expected to change)</li>
      <li>Control Inputs: (is an outside agent acting to change) None</li>
      <li>Measurement Model: (how accurate is the sensor)</li>
      <li>Update: What is the weight estimate now?</li>
    </ol>
  </li>
  <li>In this situation the Kalman filter does not have an advantage over a simple moving average</li>
  <li>Why?</li>
</ul>

<slide_break></slide_break>

<h3 id="d-scenario-train">1D Scenario: Train</h3>

<ul>
  <li>A train is on a track and moving towards the terminator (which is a block)</li>
  <li>Train has a lidar sensing the wall</li>
  <li>We have a reading the speed based on the rotation of the wheels</li>
  <li>How does the train determine where it is?</li>
  <li>Models:
    <ol>
      <li>State: Location and Velocity. Location changes a known amount each T</li>
      <li>Control Inputs: (is the driver stepping on the gas): No expected change</li>
      <li>Measurement model: (How accurate is the Lidar supposed to be)</li>
      <li>Update: What will be the new location and velocity estimate?</li>
    </ol>
  </li>
</ul>

<slide_break></slide_break>

<h3 id="kalman-train">Kalman Train</h3>

<slide_break></slide_break>

<h3 id="abstraction-of-the-kalman-filter-equations">Abstraction of the Kalman Filter Equations</h3>

<slide_break></slide_break>

<h3 id="diagram-of-kalman-filter-equations">Diagram of Kalman Filter Equations</h3>

<slide_break></slide_break>

<h3 id="kalman-gain">Kalman Gain</h3>

<ul>
  <li>Where does the Kalman Gain come from?</li>
  <li>There are complex equations which use the covariance matrix with the estimates to compute a Kalman Gain</li>
  <li>But I’ve seen examples that skip that and use a constant</li>
</ul>

<slide_break></slide_break>

<h3 id="how-to-understand">How to understand</h3>

<ul>
  <li>Most examples you find will use linear algebra (not an expert) and store things in matrices</li>
  <li>If the <code>state</code> is location and velocity in the x direction (a falling ball) that is stored in a 1x2 matrix</li>
  <li>This simplifies the expression and calculation of the formulas but it is initially confusing</li>
  <li>That is unless you are fluent in matrix and linear algebra which I am not</li>
</ul>

<slide_break></slide_break>
<p>Links for further study</p>

<ul>
  <li><a href="https://www.youtube.com/watch?v=u1QcZxWRYtg&list=PLX2gX-ftPVXU3oUFNATxGXY90AULiqnWT&index=10">Detailed Youtube on Kalman FIlters</a></li>
  <li>Learning Kalman Filters: <a href="https://simondlevy.github.io/ekf-tutorial/">The Extended Kalman Filter, an interactive tutorial</a></li>
  <li><a href="https://www.kalmanfilter.net/kalman1d.html">Kalman Filter In Depth</a></li>
  <li><a href="http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/">How a Kalman Filter Works, in pictures</a></li>
  <li><a href="https://simondlevy.github.io/ekf-tutorial/">Extended Kalman Filter Tutorial for Non-Experts</a></li>
  <li><a href="http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies">Kalman Filter for Dummies</a></li>
  <li><a href="https://medium.com/@tjosh.owoyemi/kalman-filter-predict-measure-update-repeat-20a5e618be66">Kalman Filter: Predict, Measure, Repeat</a></li>
</ul>

<slide_break></slide_break>

<h2 class="shadow p-3 bg-white rounded">Thank you. Questions?<img class="img-fluid w-100" src="https://picsum.photos/800/100.jpg" /> <small> (random Image from picsum.photos)</small></h2>