daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Changed equations to use Gu instead of Bu.

Browse Source 
I switched my code over to this nomenclature a while ago, but have not
updated the book for the switch.

As an aside, the code for the Kalman filter class in theb book is now
quite dated. I need to incorporate filterpy library more fully.
This commit is contained in:
Roger Labbe 2014-08-22 07:44:10 -07:00
parent 5590a57bb9
commit 3a89d60d6c
2 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Kalman_Filter_Math.ipynb
View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:e3462ab986f9dc643d523b8cb2a8312877c7122edff3d6d7accda49234359e80"
"signature": "sha256:9e4bdd9bceb17f11ad4da5080027fc103a05a64cc3d3ab4a8006c45d273b46c4"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -342,7 +342,7 @@
"$$\n",
"\\begin{aligned}\n",
"\\text{Predict Step}\\\\\n",
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{B u}\\;\\;\\;\\;&(1) \\\\\n",
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{G u}\\;\\;\\;\\;&(1) \\\\\n",
"\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q}\\;\\;\\;\\;&(2) \\\\\n",
"\\\\\n",
"\\text{Update Step}\\\\\n",
@ -438,9 +438,9 @@
"source": [
"Now we have the measurement steps. The first equation is\n",
"\n",
"$$\\mathbf{x}' = \\mathbf{Fx} + \\mathbf{Bu}\\tag{1}$$\n",
"$$\\mathbf{x}' = \\mathbf{Fx} + \\mathbf{Gu}\\tag{1}$$\n",
"\n",
"This is just our state transition equation which we have already discussed. $\\mathbf{Fx}$ multiplies $\\mathbf{x}$ with the state transition matrix to compute the next state. $B$ and $u$ add in the contribution of the control input $\\mathbf{u}$, if any.\n",
"This is just our state transition equation which we have already discussed. $\\mathbf{Fx}$ multiplies $\\mathbf{x}$ with the state transition matrix to compute the next state. $G$ and $u$ add in the contribution of the control input $\\mathbf{u}$, if any.\n",
"\n",
"The final equation is:\n",
"$$\\mathbf{P} = \\mathbf{FPF}^T + \\mathbf{Q}\\tag{2}$$\n",

26
Multidimensional_Kalman_Filters.ipynb
View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:16a3d9d2c42e7804eeb2791adf97a20137d37392110e1311a909efb59d06e2e5"
"signature": "sha256:99942ebb1d4b6dc04d61b47c11a57a65226da104e0a39c0145852f6e09543967"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -821,7 +821,7 @@
"\n",
"> $$\n",
"\\begin{aligned}\n",
"\\hat{\\mathbf{x}}_{t|t-1} &= \\mathbf{\\Phi_t}\\hat{\\mathbf{x}}_{t-1} + \\mathbf{B u}_t\\;\\;\\;&(1) \\\\\n",
"\\hat{\\mathbf{x}}_{t|t-1} &= \\mathbf{\\Phi_t}\\hat{\\mathbf{x}}_{t-1} + \\mathbf{G u}_t\\;\\;\\;&(1) \\\\\n",
"\\mathbf{P}_{t|t-1} &= \\mathbf{\\Phi_tP}_{t-1}\\mathbf{\\Phi}^T_t + \\mathbf{Q}_t\\;\\;\\;&(2)\n",
"\\end{aligned}\n",
"$$\n",
@ -865,7 +865,7 @@
"$$\n",
"\\begin{aligned}\n",
"\\text{Predict Step}\\\\\n",
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{B u}\\;\\;\\;&(1) \\\\\n",
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{G u}\\;\\;\\;&(1) \\\\\n",
"\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q}\\;\\;\\;&(2) \\\\\n",
"\\\\\n",
"\\text{Update Step}\\\\\n",
@ -890,7 +890,7 @@
"Look at the code below for the predict step (which we will present a bit later). \n",
"\n",
" def predict():\n",
" x = F*x + B*u # equation (1)\n",
" x = F*x + G*u # equation (1)\n",
" P = F*P*F.T + Q # equation (2)\n",
" \n",
"Notice how simple it really is. It really isn't much different from the predict step in the previous chapter, and it is a nearly exact transliteration of the equations above. As you become familiar with this notation you will find yourself able to read textbooks and paper and implement the equations without much difficulty. \n",
@ -1018,11 +1018,11 @@
"\n",
"Suppose that instead of passively tracking our dog we were actively controlling a robot. At each time step we would send control signals to the robot based on our current position vs desired position. Kalman filter equations incorporate that knowledge into the filter equations, creating a predicted position based both on current velocity *and* control inputs to the drive motors. \n",
"\n",
"We will cover this use case later, but for now passive tracking applications we set those terms to 0. In step 2 there was the unexplained term $\\mathbf{Bu}$ in equation (1):\n",
"We will cover this use case later, but for now passive tracking applications we set those terms to 0. In step 2 there was the unexplained term $\\mathbf{Gu}$ in equation (1):\n",
"\n",
"$$\\mathbf{x}' = \\mathbf{Fx} + \\mathbf{Bu}$$.\n",
"$$\\mathbf{x}' = \\mathbf{Fx} + \\mathbf{Gu}$$.\n",
"\n",
"Here $\\mathbf{u}$ is the control input, and $\\mathbf{B}$ is its transfer function. For example, $\\mathbf{u}$ might be a voltage controlling how fast the wheel's motor turns, and multiplying by $\\mathbf{B}$ yields $\\begin{smallmatrix}x\\\\\\dot{x}\\end{smallmatrix}$. Since we do not need these terms we will set them both to zero and not concern ourselves with them for now.\n"
"Here $\\mathbf{u}$ is the control input, and $\\mathbf{G}$ is its transfer function. For example, $\\mathbf{u}$ might be a voltage controlling how fast the wheel's motor turns, and multiplying by $\\mathbf{G}$ yields $\\begin{smallmatrix}x\\\\\\dot{x}\\end{smallmatrix}$. Since we do not need these terms we will set them both to zero and not concern ourselves with them for now.\n"
]
},
{
@ -1400,7 +1400,7 @@
" self.P = np.eye(dim_x) # uncertainty covariance\n",
" self.Q = np.eye(dim_x) # process uncertainty\n",
" self.u = 0 # control input vector\n",
" self.B = np.zeros((dim_x,1))\n",
" self.G = np.zeros((dim_x,1))\n",
" self.F = 0 # state transition matrix\n",
" self.H = 0 # Measurement function\n",
" self.R = np.eye(dim_z) # state uncertainty\n",
@ -1421,7 +1421,7 @@
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{B u} \\\\\n",
"\\mathbf{x}' &= \\mathbf{F x} + \\mathbf{G u} \\\\\n",
"\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q} \n",
"\\end{aligned}\n",
"$$\n",
@ -1429,7 +1429,7 @@
"The corresponding code is\n",
"\n",
" def predict(self): \n",
" self.x = self.F.dot(self.x) + self.B.dot(self.u)\n",
" self.x = self.F.dot(self.x) + self.G.dot(self.u)\n",
" self.P = self.F.dot(self.P).dot(self.F.T) + self.Q\n",
"\n",
"I haven't discussed the use of numpy much until now, but this method illustrates the power of that package. We use numpy's `array` class to store our data and perform the linear algebra for our filters. `array` implements matrix multication using the `.dot()` method; if you use `*` you will get element-wise multiplication. As a heavy user of linear algebra this design is somewhat distressing as I use matrix multiplation far more often than element-wise multiplaction. However, this design is due to historical developments in the library and we must live with it. The Python community has recognized this problem, and in Python 3.5 we will have the `@` operator to implement matrix multiplication. \n",
@ -1570,7 +1570,7 @@
" self.P = np.eye(dim_x) # uncertainty covariance\n",
" self.Q = np.eye(dim_x) # process uncertainty\n",
" self.u = np.zeros((dim_x,1)) # motion vector\n",
" self.B = 0\n",
" self.G = 0\n",
" self.F = 0 # state transition matrix\n",
" self.H = 0 # Measurement function (maps state to measurements)\n",
" self.R = np.eye(dim_z) # state uncertainty\n",
@ -1622,8 +1622,8 @@
" \"\"\" predict next position \"\"\"\n",
" \n",
" self.x = self.F.dot(self.x)\n",
" if self.B != 0:\n",
" self.x += self.B.dot(self.u)\n",
" if self.G != 0:\n",
" self.x += self.G.dot(self.u)\n",
" self.P = self.F.dot(self.P).dot(self.F.T) + self.Q"
],
"language": "python",