1. Kalman's derivation:
1) First look at the state space model of the system stochastic system: (linear)
The so-called linear means that the recursion or state transition equation is linear. 
As for the parameter explanation, read the book yourself.
2) The value at time k-1 minus the optimal state estimate at time k-1 = state estimation error at time k-1. First calculate the state estimation error at time k-1 ( the deviation between the reference value and the estimated value ):
3) According to the optimal state estimation at time k-1 and the system state equation, the state at time k (current time) can be optimally estimated ( Optimal one-step forecast):
4) Calculating the state one-step forecast error is:
5) Put 1) and 3) into 4) and combine 2) to get:
now the state one-step forecast error is calculated, then the state can be calculated One-step prediction of the mean square error matrix:
while the state estimation error matrix at k-1 time is:
Calculate and measure in the same way:
1) Through the one-step prediction of the state and the measurement equation of the system, one-step prediction of the measurement at time can be made:
2) Measurement The formula for the one-step forecast error is:
3) Bring the system formula and 1) formula into 2) get the measurement one-step forecast error:
4) Calculate the mean square error matrix of the measurement one-step forecast, the state one-step forecast and the measurement one-step forecast Co-mean square error matrix:
So far, it is possible to use the one-step prediction of the state equation of the system to estimate the value at the current moment, but since no information of the measurement equation is used, the estimation accuracy will not be high. And because the measurement one-step prediction error contains the information of the state one-step prediction, therefore, considering the comprehensive consideration, the current state estimation is corrected by the measurement one-step prediction error as the final optimal state estimation:
5) Simplify the above formula:
It can be seen that the current state estimation is a linear combination (weighted estimation) of the state estimation at the previous moment and the current measurement, and the prior estimation is corrected by the measurement error to obtain the posterior estimation.
Next, calculate K to minimize the posterior estimation error (the state estimation error at the current moment):
6) The state estimation error at the current moment k is:
7) Put the above formula 5) into 6) to get:
8) Find the moment k in the same way (Current moment) The mean square error matrix of the state estimation:
9) The estimation error is a random vector, and the meaning of "minimizing the error" is stipulated to minimize the sum of the mean square errors of each component , which is
equivalent to:
10) Because The mean square error matrix must be a symmetric matrix , so the formula 8) can be expanded as:
11) Simultaneously find the trace operation on both sides
of the formula 10), and get: the above formula is a quadratic function about the undetermined parameter matrix K, so tr§ must have a pole value (according to the meaning of probability, it should be a minimum value here).
In order to facilitate the use of the derivation method to obtain the extremum of the above formula, two equations for deriving the matrix from the trace of the square matrix are introduced, which are as follows:
Derivation of K on both sides at the same time:
According to the extremum value of the function, it is obtained:
that is, when K When it is equal to the above values, the current state estimation error is the smallest. At this point, bring the above formula into 10) to get the root mean square error matrix at time k:
So far, the five formulas of Kalman filtering are deduced:
2. Extended Kalman derivation:
1) First look at the state space model of the system stochastic system: (nonlinear)
The only difference from Kalman is that the extended Kalman deals with nonlinear models, so a Taylor expansion is required for the state transition equation (regardless of prediction or measurement) Get the Jacobian matrix for linearization.
1) Calculate the state estimation at time k-1 in the same way:
2) According to the optimal state estimation at time k-1 and the system state equation, the state at time k (current moment) can be optimally estimated (optimal one-step prediction): 3
) The one-step prediction error of the calculation state is:
In the Kalman derivation, this step is directly brought into the system equation, but here it needs to be linearized first: In the
same way, the measurement:
Therefore, the original nonlinear system can be rewritten as a linear system:
Rewrite 5 formulas:
