WEBVTT 00:00:00.058 --> 00:00:02.590 Here is the problem this lecture is about. 00:00:02.694 --> 00:00:07.313 A vehicle has to report where it is and how fast it is going, continuously, 00:00:07.396 --> 00:00:12.005 at high rate, for an hour, in open sky and under a bridge. 00:00:12.318 --> 00:00:14.174 Two instruments can answer that, 00:00:14.257 --> 00:00:17.601 and each of them fails in a way the other one does not. 00:00:18.998 --> 00:00:21.516 The first is an inertial measurement unit. 00:00:21.599 --> 00:00:25.846 Three accelerometers and three gyroscopes, bolted to the vehicle frame, 00:00:25.929 --> 00:00:30.061 reporting specific force and angular rate in their own body axes, 00:00:30.144 --> 00:00:32.257 several hundred times a second. 00:00:32.871 --> 00:00:36.495 Nothing in that list refers to the world outside the vehicle. 00:00:37.407 --> 00:00:39.079 Mechanization is the arithmetic 00:00:39.079 --> 00:00:42.633 that turns those two signals into a navigation solution. 00:00:42.829 --> 00:00:45.416 Integrate angular rate to keep attitude. 00:00:45.499 --> 00:00:49.723 Rotate the measured force into navigation axes, add gravity back, 00:00:49.806 --> 00:00:52.245 and integrate to get velocity. 00:00:52.442 --> 00:00:55.136 Integrate once more to get position. 00:00:55.933 --> 00:00:59.728 Every one of those integrals also integrates the sensor's own errors, 00:00:59.811 --> 00:01:01.657 and that is the whole trouble. 00:01:01.854 --> 00:01:07.636 A constant accelerometer bias, half a milli-g, say, becomes a velocity error 00:01:07.636 --> 00:01:12.664 that grows linearly and a position error that grows as one half b t squared. 00:01:13.576 --> 00:01:15.049 Here is what that costs, 00:01:15.132 --> 00:01:19.812 drawn against time in seconds, with metres of position error up the side. 00:01:20.009 --> 00:01:24.999 It is smooth, and it is quiet, and it climbs: past ten metres, 00:01:25.082 --> 00:01:28.438 past twenty metres, and it never comes back down. 00:01:29.351 --> 00:01:34.843 So an unaided inertial system is superb over seconds and useless over minutes. 00:01:35.040 --> 00:01:37.293 And notice which part is bad. 00:01:37.605 --> 00:01:40.219 The shape of the trajectory is excellent; 00:01:40.415 --> 00:01:43.469 it is the absolute placement that slides away. 00:01:44.982 --> 00:01:47.537 The second instrument does the opposite. 00:01:47.850 --> 00:01:51.888 A satellite receiver measures its distance to every satellite it can see, 00:01:51.971 --> 00:01:56.046 and with four or more of those distances it solves directly for three coordinates 00:01:56.046 --> 00:02:00.703 of position, and for its own clock offset, which is the fourth unknown. 00:02:01.499 --> 00:02:03.959 What comes out is an absolute fix: 00:02:04.042 --> 00:02:06.421 position, and usually velocity, 00:02:06.504 --> 00:02:11.172 expressed in an earth-fixed frame and referred to nothing but the satellites. 00:02:11.484 --> 00:02:16.291 It arrives slowly by inertial standards, one to ten times a second. 00:02:17.088 --> 00:02:18.679 And it is noisy. 00:02:18.876 --> 00:02:21.986 Receiver noise, unmodelled atmospheric delay, 00:02:22.069 --> 00:02:26.723 and signals that arrive after bouncing off a building, which is what multipath means, 00:02:26.806 --> 00:02:31.021 all move the reported fix by metres from one epoch to the next. 00:02:31.218 --> 00:02:33.506 Here it is on the same axes. 00:02:34.302 --> 00:02:36.578 But watch what it does not do. 00:02:36.682 --> 00:02:42.068 It wanders, and it keeps wandering about the same level, under about five metres, 00:02:42.151 --> 00:02:46.154 and it does not care whether you have been driving for one minute or five hours. 00:02:46.237 --> 00:02:48.200 Its error is bounded. 00:02:48.652 --> 00:02:50.731 The inertial error is not. 00:02:51.528 --> 00:02:55.592 Put them side by side and the pattern is almost too convenient. 00:02:55.905 --> 00:03:00.840 The inertial unit is fast, smooth and locally accurate, and it drifts. 00:03:00.944 --> 00:03:04.880 The receiver is slow and noisy, and it does not drift. 00:03:06.281 --> 00:03:08.394 One more line matters later. 00:03:08.591 --> 00:03:11.678 The inertial unit needs nothing outside the vehicle at all, 00:03:11.761 --> 00:03:15.463 while the receiver needs a clear view of at least four satellites, 00:03:15.546 --> 00:03:19.192 and that is the one place the scheme we are about to build gives up. 00:03:20.244 --> 00:03:23.600 So neither instrument is the answer; the pair is. 00:03:23.704 --> 00:03:27.185 And this is what the pair can do: an error that is smooth 00:03:27.268 --> 00:03:32.157 and fast like the inertial solution, and bounded like the satellite one. 00:03:32.353 --> 00:03:35.500 Building that blue curve is the whole of this lecture. 00:03:36.564 --> 00:03:38.260 So we build the pair. 00:03:38.712 --> 00:03:42.773 Loosely coupled integration is one particular way of wiring them together, 00:03:42.856 --> 00:03:46.489 and it is the one to meet first, because every piece of it can be bought, 00:03:46.572 --> 00:03:48.906 tested and replaced on its own. 00:03:49.819 --> 00:03:52.479 The top row is the inertial chain. 00:03:52.675 --> 00:03:56.702 The measurement unit hands its samples to the mechanization we just wrote down, 00:03:56.785 --> 00:03:59.326 and out comes a continuous solution: 00:03:59.409 --> 00:04:04.390 position, velocity and attitude, at the full sensor rate. 00:04:05.187 --> 00:04:08.717 Underneath, and quite separately, sits the receiver. 00:04:08.914 --> 00:04:13.370 It tracks whatever satellites it can see and solves its own navigation problem, 00:04:13.453 --> 00:04:17.657 on its own schedule, and what leaves it is a computed fix. 00:04:18.453 --> 00:04:19.836 Now the filter. 00:04:20.032 --> 00:04:22.622 It does not sit in the signal path at all. 00:04:22.726 --> 00:04:27.335 Both solutions are handed to it, and what it estimates is not where the vehicle is, 00:04:27.335 --> 00:04:30.760 but how wrong the inertial solution has become. 00:04:31.464 --> 00:04:33.996 Its measurement is therefore a difference. 00:04:34.099 --> 00:04:37.664 Take the inertial position and velocity, subtract the position 00:04:37.664 --> 00:04:41.751 and velocity the receiver reported, and what is left is error: 00:04:42.064 --> 00:04:45.629 the inertial system's error, plus the receiver's noise. 00:04:46.542 --> 00:04:49.793 And the corrections travel back up the same interface. 00:04:50.106 --> 00:04:53.135 The estimated error is written into the mechanization, 00:04:53.218 --> 00:04:57.198 and the estimated sensor biases are subtracted from the incoming samples, 00:04:57.281 --> 00:05:01.160 so the errors the filter is linearising about stay small. 00:05:01.956 --> 00:05:05.010 The word loosely is a statement about that interface. 00:05:05.323 --> 00:05:10.410 What crosses between the two systems is a finished navigation solution, six numbers. 00:05:10.513 --> 00:05:13.472 The individual satellite ranges never leave the receiver, 00:05:13.555 --> 00:05:16.563 and the receiver never hears about the inertial unit. 00:05:17.959 --> 00:05:20.921 Why estimate the error rather than the state? 00:05:21.117 --> 00:05:25.400 Look at what the error is. Here is the true path of the vehicle in grey, 00:05:25.483 --> 00:05:29.129 and here is the path the mechanization computed, in red. 00:05:29.325 --> 00:05:31.042 The shapes agree very well. 00:05:31.125 --> 00:05:32.832 The placement does not. 00:05:33.629 --> 00:05:37.911 The difference between those two paths is small, it is slowly varying, 00:05:37.994 --> 00:05:41.153 and to first order it obeys linear dynamics. 00:05:41.349 --> 00:05:44.636 That is the whole reason for the indirect formulation: 00:05:44.832 --> 00:05:49.512 the vehicle's motion is thoroughly nonlinear, but its error is not. 00:05:50.308 --> 00:05:53.909 So we do not ask the filter for the vehicle's position. 00:05:54.221 --> 00:05:58.553 We ask it for fifteen numbers that say how wrong the inertial solution is: 00:05:58.656 --> 00:05:59.885 three of position, 00:05:59.968 --> 00:06:05.878 three of velocity, three of attitude, and six sensor biases, three per triad. 00:06:06.931 --> 00:06:12.106 Those fifteen errors evolve according to a matrix F built from the current attitude, 00:06:12.189 --> 00:06:13.490 the current specific force 00:06:13.490 --> 00:06:18.320 and the bias models, plus process noise for everything F leaves out. 00:06:18.424 --> 00:06:21.409 That is the propagation half of the filter. 00:06:22.112 --> 00:06:23.469 Now the measurement. 00:06:23.552 --> 00:06:28.371 It is the inertial solution minus the receiver's solution, and nothing else. 00:06:28.568 --> 00:06:33.723 Write each of them as the truth plus its own error, and the truth cancels. 00:06:34.636 --> 00:06:38.791 What is left is the position and velocity part of the error state, 00:06:38.874 --> 00:06:40.857 minus the receiver's noise. 00:06:40.940 --> 00:06:44.595 So the measurement matrix is almost embarrassingly simple: 00:06:44.678 --> 00:06:49.079 identity on six of the fifteen states, zero on the other nine. 00:06:49.276 --> 00:06:53.259 That is the entire measurement model of a loosely coupled filter. 00:06:53.870 --> 00:06:56.170 And notice what is not in there. 00:06:56.366 --> 00:07:00.233 No satellite geometry, no ranges, no clock. 00:07:00.430 --> 00:07:04.586 All of that stayed inside the receiver, which is exactly what makes this scheme 00:07:04.586 --> 00:07:07.768 modular, and also exactly what will limit it. 00:07:08.873 --> 00:07:10.801 Now the filter itself. 00:07:11.253 --> 00:07:14.214 It alternates between two steps, and the interesting 00:07:14.214 --> 00:07:18.255 thing about this architecture is how little happens in the first of them. 00:07:19.052 --> 00:07:23.406 Between fixes there is no measurement, so nothing corrects anything at all. 00:07:23.603 --> 00:07:28.604 The mechanization runs on at full rate, the error grows exactly as it did unaided, 00:07:28.688 --> 00:07:31.857 and all the filter can do is keep track of how uncertain that growing 00:07:31.857 --> 00:07:33.437 solution has become. 00:07:34.652 --> 00:07:38.344 The prediction is the ordinary Kalman one, on the error state. 00:07:38.541 --> 00:07:42.442 Because the last correction was written straight into the mechanization, the error 00:07:42.442 --> 00:07:45.055 estimate starts each interval at zero. 00:07:45.251 --> 00:07:49.695 The transition matrix over one interval is the identity plus F delta t, 00:07:49.779 --> 00:07:53.681 and the covariance is pushed through it and grown by the process noise. 00:07:54.594 --> 00:07:56.576 Watch the picture while that runs. 00:07:56.660 --> 00:07:58.379 Nothing on it improves. 00:07:58.576 --> 00:08:02.811 Between fixes, a loosely coupled system is an unaided inertial system 00:08:02.895 --> 00:08:05.322 with a growing covariance attached to it. 00:08:06.119 --> 00:08:09.242 Then a fix arrives, and everything happens at once. 00:08:09.555 --> 00:08:11.238 The innovation is the difference we built 00:08:11.238 --> 00:08:15.790 a moment ago, less whatever the filter already expected that difference to be. 00:08:15.894 --> 00:08:19.772 Here it expected zero, so the innovation is the raw difference. 00:08:20.569 --> 00:08:23.217 The gain weighs the two sides against each other. 00:08:23.529 --> 00:08:27.474 The middle term, S, is how large a difference we should have expected, 00:08:27.558 --> 00:08:30.589 given our own uncertainty and the receiver's noise. 00:08:30.786 --> 00:08:35.349 When the receiver is noisy and the filter is confident, the correction is small. 00:08:35.662 --> 00:08:40.167 When the inertial solution has been coasting a long time, it is large. 00:08:40.964 --> 00:08:43.550 The correction is applied to the whole error state, 00:08:43.634 --> 00:08:45.980 not only to the six states we measured. 00:08:46.084 --> 00:08:48.616 Position error is observed directly. 00:08:48.813 --> 00:08:53.361 Attitude error and the gyro biases are not, and they are estimated anyway, 00:08:53.445 --> 00:08:57.428 because the covariance says they are correlated with what we did measure. 00:08:58.039 --> 00:09:01.960 And the covariance shrinks, which is the part that matters most. 00:09:02.044 --> 00:09:04.282 The filter is now more certain than it was, 00:09:04.366 --> 00:09:07.641 and the next coasting interval starts from a smaller number. 00:09:08.437 --> 00:09:10.470 Then it all repeats. 00:09:10.574 --> 00:09:13.593 And here is what to watch across several cycles: 00:09:13.789 --> 00:09:15.683 the teeth are getting shorter. 00:09:15.786 --> 00:09:20.025 Nearly nine metres on the first, under six on the next, then four. 00:09:20.128 --> 00:09:22.610 Each fix does more than reset the error; 00:09:22.694 --> 00:09:26.724 it also improves the estimate of the biases that were making it grow. 00:09:28.027 --> 00:09:30.416 That improvement deserves its own picture. 00:09:30.500 --> 00:09:34.422 This is the filter's estimate of one accelerometer bias against time, 00:09:34.506 --> 00:09:38.523 in milli-g, with the true value dashed across it at eight. 00:09:39.320 --> 00:09:42.220 It starts at zero, because we knew nothing, 00:09:42.304 --> 00:09:45.613 and it walks up to the truth over a minute or two of driving. 00:09:45.810 --> 00:09:48.921 A bias on an accelerometer produces a velocity error 00:09:48.921 --> 00:09:53.566 that grows in a particular direction, and every fix reports that direction again. 00:09:53.763 --> 00:09:57.757 So the filter is calibrating the instrument while the vehicle drives. 00:09:58.670 --> 00:10:01.686 Which brings us to where the correction is actually applied, 00:10:01.770 --> 00:10:03.616 and there are two choices. 00:10:03.813 --> 00:10:08.094 In the feedback configuration the estimate is written into the mechanization: 00:10:08.178 --> 00:10:11.868 position, velocity and attitude are overwritten, 00:10:11.952 --> 00:10:13.902 the estimated biases are subtracted 00:10:13.902 --> 00:10:18.616 from the incoming samples, and the error estimate is then reset to zero. 00:10:19.413 --> 00:10:23.741 In the feedforward configuration the mechanization is left running untouched, 00:10:23.825 --> 00:10:26.751 and the correction is added to its output instead. 00:10:26.948 --> 00:10:31.151 That is easier to bolt onto an existing box, but the inertial solution keeps 00:10:31.151 --> 00:10:36.028 drifting underneath, so the errors the filter linearised about stop being small. 00:10:36.225 --> 00:10:38.211 Feedback keeps them small. 00:10:39.007 --> 00:10:44.163 So the loop is four words: coast, compare, correct, calibrate. 00:10:44.360 --> 00:10:47.367 And note where the navigation solution comes from. 00:10:47.564 --> 00:10:50.092 It is produced entirely by the mechanization; 00:10:50.176 --> 00:10:54.171 the filter never computes a position, only a correction to one. 00:10:55.314 --> 00:11:00.551 Two things left: what this arrangement buys, and the one place it stops working. 00:11:00.748 --> 00:11:03.825 The second is easier to see, so start there. 00:11:03.929 --> 00:11:08.072 On the left, the number of satellites the receiver can use; on the right, 00:11:08.155 --> 00:11:11.813 the error of the integrated solution over the same half minute. 00:11:12.424 --> 00:11:15.232 With six satellites in view the fixes keep arriving, 00:11:15.315 --> 00:11:19.042 once a second, and the error stays well under five metres: 00:11:19.146 --> 00:11:22.897 the sawtooth we have just been watching, on a coarser scale. 00:11:23.693 --> 00:11:27.384 Then the vehicle turns into a street with tall buildings on both sides, 00:11:27.467 --> 00:11:29.906 and the usable count falls to three. 00:11:30.102 --> 00:11:34.628 Four independent ranges are needed for three coordinates and the clock offset, 00:11:34.711 --> 00:11:37.986 so with three the receiver publishes nothing at all. 00:11:38.783 --> 00:11:42.522 And a loosely coupled filter has nothing to do with three ranges. 00:11:42.625 --> 00:11:47.163 Its measurement is a position and velocity solution, and there is no solution, 00:11:47.246 --> 00:11:52.053 so there is no update: no innovation, no gain, no correction. 00:11:52.505 --> 00:11:53.885 The filter coasts, 00:11:53.968 --> 00:11:57.916 and what you are watching is once again the unaided inertial error. 00:11:58.457 --> 00:12:02.531 Ten seconds of that, and the solution is nearly twenty metres out. 00:12:02.614 --> 00:12:05.387 Then the street opens, the satellites come back, 00:12:05.470 --> 00:12:08.896 and the very first fix pulls the whole thing down again. 00:12:08.999 --> 00:12:13.040 The recovery is immediate: the filter kept a valid covariance the whole way 00:12:13.040 --> 00:12:17.011 through the gap, so it knows exactly how little to trust itself. 00:12:18.315 --> 00:12:21.404 So here is the trade, and it is a clear one. 00:12:21.716 --> 00:12:26.951 On the gain side: the receiver is a box you can buy, qualify and replace, 00:12:27.034 --> 00:12:29.728 and the filter never touches its internals. 00:12:30.180 --> 00:12:32.942 The measurement model is six states of identity, 00:12:33.025 --> 00:12:35.751 so the code is small and the update is cheap. 00:12:35.834 --> 00:12:38.691 And you have two solutions that can check each other. 00:12:39.488 --> 00:12:43.224 On the other side: the receiver has already filtered its own output, 00:12:43.307 --> 00:12:45.442 so consecutive fixes are correlated 00:12:45.525 --> 00:12:48.683 and the noise covariance you assume is optimistic. 00:12:48.880 --> 00:12:50.678 The fix is old when it arrives, 00:12:50.761 --> 00:12:54.475 so it must be time-tagged and applied to the instant it belongs to. 00:12:54.558 --> 00:12:57.983 And below four satellites there is no measurement at all. 00:12:58.687 --> 00:13:01.753 None of that is a reason to avoid the architecture. 00:13:01.857 --> 00:13:04.830 It is a reason to know which failure you are buying. 00:13:04.933 --> 00:13:10.495 This filter is exactly as available as the receiver's position solution, and no more. 00:13:11.110 --> 00:13:13.639 If you need aiding below four satellites, 00:13:13.722 --> 00:13:18.228 you need a different interface, and that is what a tightly coupled design is for. 00:13:19.624 --> 00:13:21.866 So, the whole thing in one line. 00:13:22.062 --> 00:13:25.033 The navigation solution is the mechanization's own, 00:13:25.116 --> 00:13:29.736 corrected by an estimate of its own error, and that estimate is driven by nothing 00:13:29.736 --> 00:13:34.068 more than the difference between two independent solutions to the same problem. 00:13:34.864 --> 00:13:38.961 The inertial unit supplies continuity, smoothness and rate. 00:13:39.044 --> 00:13:42.028 The receiver supplies an absolute reference. 00:13:42.480 --> 00:13:46.927 And the error-state filter in between carries the calibration from one fix 00:13:46.927 --> 00:13:51.420 to the next, which is why the solution keeps its accuracy through the ten seconds 00:13:51.420 --> 00:13:53.139 when nobody is checking it.