Bug fixes and added example.
Several of the equations were slightly wrong. I made variables bold when required, and added an example. I also expanded some of the explanations.
This commit is contained in:
Roger Labbe committed
1 parent
8566445bfb
commit
203616ff1c
1 file changed
+175
-29
@@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:64766d1e76106fc2b6471e25399e4cb6fa3a725d0de4cc1e0aecb96437b2b0be"
|
||||
"signature": "sha256:2b626ac51cd84e0593614b830cc5ea8b9d2af5ca92e0863eaba39906bb9cd0af"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@@ -261,7 +261,7 @@
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x7f5cc95696a0>"
|
||||
"<IPython.core.display.HTML at 0x7fc25989a588>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -275,7 +275,7 @@
|
||||
"\n",
|
||||
"The ensemble Kalman filter (EnKF) is very similar to the unscented Kalman filter (UKF) of the last chapter. If you recall, the UKF uses a set of deterministically chosen weighted sigma points passed through nonlinear state and measurement functions. After the sigma points are passed through the function, we find the mean and covariance of the points and use this as the filter's new mean and covariance. It is only an approximation of the true value, and thus suboptimal, but in practice the filter is highly accurate. It has the advantage of often producing more accurate estimates than the EKF does, and also does not require you to analytically derive the linearization of the state and measurement equations. \n",
|
||||
"\n",
|
||||
"The ensemble Kalman filter works in a similar way, except it uses a *Monte Carlo* method to choose the sigma points. The filter starts by randomly generating a large number of points distributed about the filter's initial state. This distribution is proportional to the filter's covariance $\\mathbf{P}$. In other words 68% of the points will be within one standard deviation of the mean, 95% percent within two standard deviations, and so on. Let's look at this in two dimensions. We will use `numpy.random.multivariate_normal()` function to randomly create points from a multivariate normal distribution drawn from the mean (5, 3) with the covariance\n",
|
||||
"The ensemble Kalman filter works in a similar way, except it uses a *Monte Carlo* method to choose a large numbers of sigma points. It came about from the geophysical sciences as an answer for the very large states and systems needed to model things such as the ocean. The filter starts by randomly generating a large number of points distributed about the filter's initial state. This distribution is proportional to the filter's covariance $\\mathbf{P}$. In other words 68% of the points will be within one standard deviation of the mean, 95% percent within two standard deviations, and so on. Let's look at this in two dimensions. We will use `numpy.random.multivariate_normal()` function to randomly create points from a multivariate normal distribution drawn from the mean (5, 3) with the covariance\n",
|
||||
"\n",
|
||||
"$$\\begin{bmatrix}\n",
|
||||
"32 & 15 \\\\ 15 & 40\n",
|
||||
@@ -301,7 +301,7 @@
|
||||
"plt.scatter(x, y, alpha=0.3, marker='.')\n",
|
||||
"plt.axis('equal')\n",
|
||||
"\n",
|
||||
"plot_covariance_ellipse(mean=mean, cov=p,\n",
|
||||
"plot_covariance_ellipse(mean=mean, cov=P,\n",
|
||||
" variance=2.**2,\n",
|
||||
" facecolor='none')"
|
||||
],
|
||||
@@ -311,13 +311,13 @@
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAsQAAAF2CAYAAABpv45gAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAsQAAAF2CAYAAABpv45gAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f5ca4cf0198>"
|
||||
"<matplotlib.figure.Figure at 0x7fc23c43b780>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 6
|
||||
"prompt_number": 2
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@@ -333,7 +333,7 @@
|
||||
"\n",
|
||||
"**Initialize Step**\n",
|
||||
"\n",
|
||||
"$$\\chi \\sim \\mathcal{N}(\\mathbf{x}_0, \\mathbf{P}_0)\n",
|
||||
"$$\\boldsymbol\\chi \\sim \\mathcal{N}(\\mathbf{x}_0, \\mathbf{P}_0)\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"This just says to select the sigma points from the filter's initial mean and covariance. In code this might look like\n",
|
||||
@@ -350,8 +350,8 @@
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"\\chi &= f(\\chi, u) + v_Q \\\\\n",
|
||||
"x &= \\frac{1}{N} \\sum_1^N \\chi\n",
|
||||
"\\boldsymbol\\chi &= f(\\boldsymbol\\chi, \\mathbf{u}) + v_Q \\\\\n",
|
||||
"\\mathbf{x} &= \\frac{1}{N} \\sum_1^N \\boldsymbol\\chi\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"That is short and sweet, but perhaps not entirely clear. The first line passes all of the sigma points through a use supplied state transition function and then adds some noise distributed according to the $\\mathbf{Q}$ matrix. In Python we might write"
|
||||
@@ -382,9 +382,9 @@
|
||||
"source": [
|
||||
"We can now optionally compute the covariance of the mean. The algorithm does not need to compute this value, but it is often useful for analysis. The equation is\n",
|
||||
"\n",
|
||||
"$$\\mathbf{P} = \\frac{1}{N-1}\\sum_1^N[\\chi-\\mathbf{x}^-][\\chi-\\mathbf{x}^-]^\\mathsf{T}$$\n",
|
||||
"$$\\mathbf{P} = \\frac{1}{N-1}\\sum_1^N[\\boldsymbol\\chi-\\mathbf{x}^-][\\boldsymbol\\chi-\\mathbf{x}^-]^\\mathsf{T}$$\n",
|
||||
"\n",
|
||||
"$\\chi-\\mathbf{x}^-$ is a one dimensional vector, so we will use `numpy.outer` to compute the $[\\chi-\\mathbf{x}^-][\\chi-\\mathbf{x}^-]^\\mathsf{T}$ term. In Python we might write\n",
|
||||
"$\\boldsymbol\\chi-\\mathbf{x}^-$ is a one dimensional vector, so we will use `numpy.outer` to compute the $[\\boldsymbol\\chi-\\mathbf{x}^-][\\boldsymbol\\chi-\\mathbf{x}^-]^\\mathsf{T}$ term. In Python we might write\n",
|
||||
"\n",
|
||||
" P = 0\n",
|
||||
" for s in sigmas:\n",
|
||||
@@ -402,13 +402,15 @@
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"\\chi_h &= h(\\chi, u)\\\\\n",
|
||||
"z_{mean} &= \\frac{1}{N}\\sum_1^N \\chi_h \\\\ \\\\\n",
|
||||
"P_{zz} &= \\frac{1}{N-1}\\sum_1^N [\\chi_h - z_{mean}][\\chi_h - z_{mean}]^\\mathsf{T} + R \\\\\n",
|
||||
"P_{xz} &= \\frac{1}{N-1}\\sum_1^N [\\chi - x^-][\\chi_h - z_{mean}]^\\mathsf{T} \\\\\n",
|
||||
"\\boldsymbol\\chi_h &= h(\\boldsymbol\\chi, u)\\\\\n",
|
||||
"\\mathbf{z}_{mean} &= \\frac{1}{N}\\sum_1^N \\boldsymbol\\chi_h \\\\ \\\\\n",
|
||||
"\\mathbf{P}_{zz} &= \\frac{1}{N-1}\\sum_1^N [\\boldsymbol\\chi_h - \\mathbf{z}_{mean}][\\boldsymbol\\chi_h - \\mathbf{z}_{mean}]^\\mathsf{T} + \\mathbf{R} \\\\\n",
|
||||
"\\mathbf{P}_{xz} &= \\frac{1}{N-1}\\sum_1^N [\\boldsymbol\\chi - \\mathbf{x}^-][\\boldsymbol\\chi_h - \\mathbf{z}_{mean}]^\\mathsf{T} \\\\\n",
|
||||
"\\\\\n",
|
||||
"K &= P_{xz} P_{zz}^{-1}\\\\ \n",
|
||||
"x & = x + K[z - v_R + \\chi_h]\n",
|
||||
"\\mathbf{K} &= \\mathbf{P}_{xz} \\mathbf{P}_{zz}^{-1}\\\\ \n",
|
||||
"\\boldsymbol\\chi & = \\boldsymbol\\chi + \\mathbf{K}[\\mathbf{z} -\\boldsymbol\\chi_h + \\mathbf{v}_R] \\\\ \\\\\n",
|
||||
"\\mathbf{x} &= \\frac{1}{N} \\sum_1^N \\boldsymbol\\chi \\\\\n",
|
||||
"\\mathbf{P} &= \\mathbf{P} - \\mathbf{KP}_{zz}\\mathbf{K}^\\mathsf{T}\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$"
|
||||
]
|
||||
@@ -421,7 +423,7 @@
|
||||
"\n",
|
||||
"The first line,\n",
|
||||
"\n",
|
||||
"$$\\chi_h = h(\\chi, u),$$\n",
|
||||
"$$\\boldsymbol\\chi_h = h(\\boldsymbol\\chi, u),$$\n",
|
||||
"\n",
|
||||
"just passes the sigma points through the measurement function $h$. We name the resulting points $\\chi_h$ to distinguish them from the sigma points. In Python we could write this as\n",
|
||||
"\n",
|
||||
@@ -433,9 +435,9 @@
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The next line computes the mean of the measurement sigmas.\n",
|
||||
"$$z_{mean} = \\frac{1}{N}\\sum_1^N \\chi_h$$\n",
|
||||
"$$\\mathbf{z}_{mean} = \\frac{1}{N}\\sum_1^N \\boldsymbol\\chi_h$$\n",
|
||||
"\n",
|
||||
"In Python we can write\n",
|
||||
"In Python we can compute that with\n",
|
||||
"\n",
|
||||
" z_mean = np.mean(sigmas_h, axis=0)\n",
|
||||
" \n",
|
||||
@@ -443,8 +445,8 @@
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"P_{zz} &= \\frac{1}{N-1}\\sum_1^N [\\chi_h - z_{mean}][\\chi_h - z_{mean}]^\\mathsf{T} + R \\\\\n",
|
||||
"P_{xz} &= \\frac{1}{N-1}\\sum_1^N [\\chi - x^-][\\chi_h - z_{mean}]^\\mathsf{T}\n",
|
||||
"\\mathbf{P}_{zz} &= \\frac{1}{N-1}\\sum_1^N [\\boldsymbol\\chi_h - \\mathbf{z}_{mean}][\\boldsymbol\\chi_h - \\mathbf{z}_{mean}]^\\mathsf{T} + \\mathbf{R} \\\\\n",
|
||||
"\\mathbf{P}_{xz} &= \\frac{1}{N-1}\\sum_1^N [\\boldsymbol\\chi - \\mathbf{x}^-][\\boldsymbol\\chi_h - \\mathbf{z}_{mean}]^\\mathsf{T}\n",
|
||||
"\\end{aligned}$$\n",
|
||||
"\n",
|
||||
"We can express this in Python with\n",
|
||||
@@ -465,14 +467,148 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Computation of the Kalman gain is straightforward $K = P_{xz} P_{zz}^{-1}$.\n",
|
||||
"Computation of the Kalman gain is straightforward $\\mathbf{K} = \\mathbf{P}_{xz} \\mathbf{P}_{zz}^{-1}$.\n",
|
||||
"\n",
|
||||
"In Python this is the trivial\n",
|
||||
"\n",
|
||||
" K = np.dot(P_xz, inv(P_zz))\n",
|
||||
"\n",
|
||||
"Finally, we compute the new state \n",
|
||||
"x & = x + K[z - v_R + \\chi_h]"
|
||||
"Next, we update the sigma points with\n",
|
||||
"\n",
|
||||
"$$\\boldsymbol\\chi = \\boldsymbol\\chi + \\mathbf{K}[\\mathbf{z} -\\boldsymbol\\chi_h + \\mathbf{v}_R]$$ \n",
|
||||
"\n",
|
||||
"Here $\\mathbf{v}_R$ is the perturbation that we add to the sigmas. In Python we can implment this with\n",
|
||||
"\n",
|
||||
" v_r = multivariate_normal([0]*dim_z, R, N)\n",
|
||||
" for i in range(N):\n",
|
||||
" sigmas[i] += dot(K, z + v_r[i] - sigmas_h[i])\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Our final step is recompute the filter's mean and covariance.\n",
|
||||
"\n",
|
||||
" x = np.mean(sigmas, axis=0)\n",
|
||||
" P = self.P - dot3(K, P_zz, K.T)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Implementation and Example\n",
|
||||
"\n",
|
||||
"I have implemented an EnKF in the `FilterPy` library. It is in many ways a toy. Filtering with a large number of sigma points gives us very slow performance. Furthermore, there are many minor variations on the algorithm in the literature. I wrote this mostly because I was interested in learning a bit about the filter. I have not used it for a real world problem, and I can give no advice on using the filter for the large problems for which it is suited. Therefore I will refine my comments to implementing a very simple filter. I will use it to track an object in one dimension, and compare the output to a linear Kalman filter. This is a filter we have designed many times already in this book, so I will design it with little comment. Our state vector will be\n",
|
||||
"\n",
|
||||
"$$\\mathbf{x} = \\begin{bmatrix}x\\\\ \\dot{x}\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"The state transition function is\n",
|
||||
"\n",
|
||||
"$$\\mathbf{F} = \\begin{bmatrix}1&1\\\\0&1\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"and the measurement function is\n",
|
||||
"$$\\mathbf{H} = \\begin{bmatrix}1&0\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"The EnKF is designed for nonlinear problems, so instead of using matrices to implement the state transition and measurement functions you will need to supply Python functions. For this problem they can be written as:\n",
|
||||
"\n",
|
||||
" def hx(x):\n",
|
||||
" return np.array([x[0]])\n",
|
||||
"\n",
|
||||
" def fx(x, dt):\n",
|
||||
" return np.dot(F, x)\n",
|
||||
" \n",
|
||||
"One final thing: the EnKF code, like the UKF code, uses a single dimension for $\\mathbf{x}$, not a two dimensional column matrix as used by the linear kalman filter code.\n",
|
||||
"\n",
|
||||
"Without further ado, here is the code."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"from numpy.random import randn\n",
|
||||
"from filterpy.kalman import EnsembleKalmanFilter as EnKF\n",
|
||||
"from filterpy.kalman import KalmanFilter\n",
|
||||
"from filterpy.common import Q_discrete_white_noise\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"np.random.seed(1234)\n",
|
||||
"\n",
|
||||
"def hx(x):\n",
|
||||
" return np.array([x[0]])\n",
|
||||
"\n",
|
||||
"def fx(x, dt):\n",
|
||||
" return np.dot(F, x)\n",
|
||||
" \n",
|
||||
"F = np.array([[1., 1.],[0., 1.]])\n",
|
||||
"\n",
|
||||
"x = np.array([0., 1.])\n",
|
||||
"P = np.eye(2)* 100.\n",
|
||||
"enf = EnKF(x=x, P=P, dim_z=1, dt=1., N=20, hx=hx, fx=fx)\n",
|
||||
"\n",
|
||||
"std_noise = 10.\n",
|
||||
"enf.R *= std_noise**2\n",
|
||||
"enf.Q = Q_discrete_white_noise(2, 1., .001)\n",
|
||||
"\n",
|
||||
"kf = KalmanFilter(dim_x=2, dim_z=1)\n",
|
||||
"kf.x = np.array([x]).T\n",
|
||||
"kf.F = F.copy()\n",
|
||||
"kf.P = P.copy()\n",
|
||||
"kf.R = f.R.copy()\n",
|
||||
"kf.Q = f.Q.copy()\n",
|
||||
"kf.H = np.array([[1., 0.]])\n",
|
||||
"\n",
|
||||
"measurements = []\n",
|
||||
"results = []\n",
|
||||
"ps = []\n",
|
||||
"kf_results = []\n",
|
||||
"\n",
|
||||
"zs = []\n",
|
||||
"for t in range (0,100):\n",
|
||||
" # create measurement = t plus white noise\n",
|
||||
" z = t + randn()*std_noise\n",
|
||||
" zs.append(z)\n",
|
||||
"\n",
|
||||
" enf.predict()\n",
|
||||
" enf.update(np.asarray([z]))\n",
|
||||
" \n",
|
||||
" kf.predict()\n",
|
||||
" kf.update(np.asarray([[z]]))\n",
|
||||
"\n",
|
||||
" # save data\n",
|
||||
" results.append (f.x[0])\n",
|
||||
" kf_results.append (kf.x[0,0])\n",
|
||||
" measurements.append(z)\n",
|
||||
" ps.append(3*(f.P[0,0]**.5))\n",
|
||||
"\n",
|
||||
"results = np.asarray(results)\n",
|
||||
"ps = np.asarray(ps)\n",
|
||||
"\n",
|
||||
"plt.plot(results, label='EnKF')\n",
|
||||
"plt.plot(kf_results, label='KF', c='g')\n",
|
||||
"plt.plot(measurements, c='r', label='z')\n",
|
||||
"plt.plot (results-ps, c='k',linestyle='--', label='3$\\sigma$')\n",
|
||||
"plt.plot(results+ps, c='k', linestyle='--')\n",
|
||||
"plt.legend(loc='best')\n",
|
||||
"plt.show()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAs4AAAF2CAYAAAB+nR6pAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7fc234731208>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 13
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"It can be a bit difficult to see, but the KF and EnKF start off slightly different, but soon converge to producing nearly the same values. The EnKF is a suboptimal filter, so it will not produce the optimal solution that the KF produces. However, I deliberately chose $N$ to be quite small (20) to guarantee that the EnKF output is quite suboptimal. If I chose a more reasonable number such as 2000 you would be unable to see the difference between the two filter outputs on this graph."
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -489,7 +625,7 @@
|
||||
"\n",
|
||||
"The same equation in Crassidis [2] reads (I'll use the same notation as in Brown, although Crassidis' is different)\n",
|
||||
"\n",
|
||||
"$$ \\hat{x} = \\frac{1}{N}\\sum_{i=1}^N\\chi_k^{(i)}$$\n",
|
||||
"$$ \\hat{x} = \\frac{1}{N-1}\\sum_{i=1}^N\\chi_k^{(i)}$$\n",
|
||||
"\n",
|
||||
"The same is true in both sources for the sums in the computation for the covariances. Crassidis, in the context of talking about the filter's covariance, states that $N-1$ is used to ensure an unbiased estimate. Given the following standard equations for the mean and standard deviation (p.2 of Crassidis), this makes sense for the covariance.\n",
|
||||
"\n",
|
||||
@@ -499,14 +635,24 @@
|
||||
" \\sigma^2 &= \\frac{1}{N-1}\\sum_{i=1}^N\\{[\\tilde{z}(t_i) - \\hat{z}(t_i)] - \\mu\\}^2\n",
|
||||
"\\end{aligned}\n",
|
||||
" $$\n",
|
||||
" \n",
|
||||
"\n",
|
||||
"However, I see no justification or reason to use $N-1$ to compute the mean. If I use $N-1$ in the filter for the mean the filter does not converge and the state essentially follows the measurements without any filtering. However, I do see a reason to use it for the covariance as in Crassidis, in contrast to Brown. Again, I support my decision emperically - $N-1$ works in the implementation of the filter, $N$ does not.\n",
|
||||
"\n",
|
||||
"My second question relates to the use of the $\\mathbf{R}$ matrix. In Brown $\\mathbf{R}$ is added to $\\mathbf{P}_{zz}$ whereas it isn't in Crassidis and other sources. I have read on the web notes by other implementers tthat adding R helps the filter, and it certainly seems reasonable and necessary to me, so this is what I do. \n",
|
||||
"\n",
|
||||
"My third question relates to the computation of the covariance $\\mathbf{P}$. Again, we have different equations in Crassidis and Brown. I have chosen the implementation given in Brown as it seems to give me the behavior that I expect (convergence of $\\mathbf{P}$ over time) and it closely compares to the form in the linear KF. In contrast I find the equations in Crassidis do not seem to converge much.\n",
|
||||
"\n",
|
||||
"I am not comfortable saying either book is wrong; it is quite likely that I missed some point that makes each set of equations work. I can say that when I implemented them as written I did not get a filter that worked. Between reading implementation notes on the web and reasoning about various issues I have chosen the implementation in this chapter, which does in fact seem to work correctly. I have yet to explore the significant amount of original literature that will likely definitively explain the discrepencies. I would like to leave this here in some form even if I do find an explanation that reconciles the various differences, as if I got confused by these books than probably others will as well.\n",
|
||||
"My fourth question relates to the state estimate update. In Brown we have \n",
|
||||
"\n",
|
||||
"$$\\boldsymbol\\chi = \\boldsymbol\\chi + \\mathbf{K}[\\mathbf{z} -\\mathbf{z}_{mean} + \\mathbf{v}_R]$$ \n",
|
||||
"\n",
|
||||
"whereas in Crassidis we have\n",
|
||||
"\n",
|
||||
"$$\\boldsymbol\\chi = \\boldsymbol\\chi + \\mathbf{K}[\\mathbf{z} -\\boldsymbol\\chi_h + \\mathbf{v}_R]$$ \n",
|
||||
"\n",
|
||||
"To me the Crassidis equation seems logical, and it produces a filter that performs like the linear KF for linear problems, so that is the formulation that I have chosen. \n",
|
||||
"\n",
|
||||
"I am not comfortable saying either book is wrong; it is quite possible that I missed some point that makes each set of equations work. I can say that when I implemented them as written I did not get a filter that worked. I define \"work\" as performs essentially the same as the linear KF for linear problems. Between reading implementation notes on the web and reasoning about various issues I have chosen the implementation in this chapter, which does in fact seem to work correctly. I have yet to explore the significant amount of original literature that will likely definitively explain the discrepencies. I would like to leave this here in some form even if I do find an explanation that reconciles the various differences, as if I got confused by these books than probably others will as well.\n",
|
||||
"\n",
|
||||
"## References\n",
|
||||
"- [1] Crassidis, John L., and John L. Junkins. *Optimal estimation of dynamic systems*. CRC press, 2011.\n",
|
||||
|
||||
Reference in new issue
Block a user