Understanding Kalman filtering in simple terms

Important note: This article is compiled from online information. It only records the blogger's process of learning relevant knowledge points and has not been deleted.

1. Reference materials

The Kalman filter I understand
is about Kalman filter, an easy-to-understand tutorial
for a simple analysis of Kalman
filterHow a Kalman filter works, in
picturesTalk about Kalman filterExtended
Kalman Filter (EKF) With Python Code ExampleKalman
filter Kalman Filter in
Kalman Filter Algorithm SLAM Derivation of Mobile Robot Technology (7.5) -- Kalman Filter and Extended Kalman Filter for State Estimation

video material

From giving up to mastering! Kalman filtering from theory to practice~
[Kalman Filter]_Kalman Filter_The most detailed mathematical derivation on the entire network.
How to explain Kalman filtering in as simple and detailed a way as possible?

2. Related introduction

Dirichlet Distribution of probability distribution (prior, posterior, likelihood)

1. Probability distribution

Take radar ranging as an example. Radar measurement finds that the missile has a probability of 0.8 at a position of 7.2m, a probability of 0.1 at a position of 7.2m, and a probability of 0.1 at a position of 6.9m. These data are called probability distributions. Data consisting of a number of values ​​and the probabilities of those values ​​occurring is called a probability distribution .

2. Noise error

When using the compass software on the mobile phone, the compass will still fluctuate within a certain range, although it is obviously not moving; when using the scale, it is obviously not moving, but the data of the scale keeps changing. The above phenomena are noise errors. LiDAR, millimeter-wave radar, temperature sensors, etc. all have noise errors during use.

• The probability distribution of the distance between the missile and the target measured by the radar at time t is $z_t=N(7,0.1^2)$, 0.1 is the radar noise error.
• The probability distribution of the position of the missile at time t-1 is $x_{t-1}=N(10,0.2^2)$, 0.2 is the noise error of the missile position at the last moment.

Kalman filtering is used to reduce noise errors by fusing radar measurements and missile state data from the previous moment. It is mainly divided into two steps:

1. Based on the missile position in the last secondestimated valueand the speed of the missile (assuming uniform motion), roughly estimate the position of the missile at the current momentestimated value;
   $$Rough\; estimate\; ofxattime\; t=x_{t-1}-v_{t-1}=N(10,0.2^2)-N(4,0.7^2)=N(6,0.2^2+0.7^2)$$

2. Missile position measured by radarMeasurementsand a rough estimate of the missile’s location.estimated value, perform linear weighting to obtain an accurate estimate of the missile position. This value is calledbest estimate。

   Simply multiply these two probability distributions to get the probability distribution of the exact estimate.
   $$x_t=\text{Rough estimate of}x\text{at}time\; t\times z_t=N(6,0.2^2+0.7^2)\times N(7,0.1^2)$$

   $$\begin{array}{rl}& N(6,0.2^2+0.7^2)\times N(7,0.1^2)=N(6,0.53^2)\times N(7,0.1^2)\\ & =N(\frac{6\ast 0.1^2+7\ast 0.53^2}{0.53^2+0.1^2},\frac{0.1^2\ast 0.53^2}{0.53^2+0.1^2})\end{array}$$

   $$\begin{array}{rl}& x_t=N(\frac{6\ast 0.1^2+7\ast 0.53^2}{0.53^2+0.1^2},\frac{0.1^2\ast 0.53^2}{{\left(0.53^2+0.1^2\right)}^2})=N((1-\frac{0.53^2}{0.53^2+0.1^2})\ast 6+\frac{0.53^2}{0.53^2+0.1^2}\ast 7,\\ & \frac{0.53^2+0.1^2}{{\left(0.53^2+0.1^2\right)}^2})\end{array}$$

   Among them, $\frac{0.53^2}{0.53^2+0.1^2}$calledKalman gain。

3. Gaussian distribution

Gaussian distribution is also known asnormal distribution，记的$\text{N}(\mu ,p^2)$, among$m,\sigma$ is the parameter of normal distribution, and they are the expectation and variance of Gaussian distribution respectively. When these two parameters are determined, the probability$p\left(x\right)$ is also determined. In particular, when$m=0,p^2=When\; 1$ , the distribution ofstandard normal distribution. Gaussian distribution has the highest probability at the mean position.

Gaussian distribution of random variables, the probability density function of which is:
$$\mathcal{N}(x,m,s)=\frac{1}{p\sqrt{2p.m}}{e}^{-\frac{(x-\mu {)}^2}{2p^2}}$$

3.1 Mean and variance

In the normal distribution, each random variable Variance represents the degree of uncertainty in a variable. Variance is used as credibility, and the larger the variance, the less credible it is.

Mean $\mu$ is the mathematical expectation$m=E(X)$，方差 $D(X)=p^2=\mathrm{And}\{[x-E\left(x\right){]}^2\}$is a measure of the dispersion of random variables. The square root of the variance is the standard deviation, which is$s$ .

3.2 Covariance

Introduction to Covariance Matrix (Covariance Matrix)

Covariance describes the degree of correlation between two variables. The mathematical expression is:
$$\begin{array}{rl}Cov\_\left(X,Y\right)& =E\left[(X-E\left[X\right])\left(Y-E\left[Y\right]\right)\right]\\ & =E\left[XY\right]-2E\_\left[Y\right]E\left[X\right]+E\left[X\right]E\left[Y\right]\\ & =E\left[XY\right]-E\left[X\right]E\left[Y\right]\end{array}$$

3.3 Gaussian distribution multiplication formula

$$N(m_1,p_1^2)\ast N(m_2,p_2^2)=N(\frac{p_1^2{m}_2+p_2^2{m}_1}{p_1^2+p_2^2},\frac{p_1^2{p}_2^2}{p_1^2+p_2^2})$$

4. Bayesian formula

$$\text{Posterior probability}p(\theta |x)=\frac{\text{Prior probability}p\left(\theta \right)\ast \text{Likelihood}p(x|\theta }{\text{Total probability}P\left(x\right)}$$

Among them, the total probability $P\left(x\right)$ is the same as$\theta$ is an independent constant, so the posterior probability is proportional to the product of the prior and the likelihood.

5. Prior probability vs posterior probability

The prior probability is a pre-objective assessment of the possibility of a certain event happening, and the posterior probability is the probability that something that has happened was caused by a certain cause.

5.1 Prior probability (prior)

Prior probability $p\left(\theta \right)$ : The event has not occurred, find the random variable$The\; probability\; that\; \theta$ occurs.

5.2 Posterior probability (posterior)

Posterior probability $p(\theta |x)$ : event$x$ has occurred, find the random variable$The\; probability\; that\; \theta$ occurs.

5.3 Likelihood

Likelihood probability $p(x|\theta )$ : The likelihood function is a function of parameters that describes whether the model parameters are reasonable given a specific observation value. That is,From "cause" to the possibility of "effect"。

5.4 Examples

There are three random events A, B, and C, and the possible outcome is D.

• A: Rain;
• B: Traffic jam;
• C: Got up late;
• D: Late.

The probability of occurrence of events A, B and C $P(A),P(B),P\left(C\right)$ isPriori probability, the prior probability is independent of the late outcome.

A late event has occurred, which event (A, B, C) caused by the probability, respectively recorded as $P(A|D),P(B|D),P(C|D)$ , calledPosterior probability。

When the event (A, B, C) occurs, the description of D's lateness is recorded as $P(D|A),P(D|B),P(D|C)$ , calledlikelihood probability. Which of the three events is more reasonable to attribute the lateness to rain, traffic jams, and getting up late? The most reasonable answer is what we often saymaximum likelihood。

6. Observed vs Predicted

predicted value, also known asvaluation, is the mathematical model prediction value (Prediction).

observed value, also known asactual value, is the sensor measurement value (Measurement), for example: lidar directly measures the distance of 7m between the robot and the obstacle.

7. Filtering and Filter (Filter)

filtering is weighting(Filtering is weighting). The function of filtering is to give different weights to different signal components . The simplest loss pass filteris to directly give the low-frequency signal a weight of 1, and give the high-frequency part a weight of 0. For more complex filtering, such as Wiener filtering, the weights need to be designed based on the statistical knowledge of the signal.

From the perspective of statistical signal processing, noise reduction can be regarded as a type of filtering. The purpose of noise reduction is to highlight the signal itself and suppress the influence of noise. From this perspective, noise reduction means giving a high weight to the signal and a low weight to the noise. Wiener filter is a typical noise reduction filter.

7.1 Filtering algorithm in the field of robotics

The filtering algorithm in the field of robotics refers to estimating the true position of the robot from noisy robot position data . Commonly used filtering algorithms include Bayes filtering and Kalman filtering.

According to the movement model (mathematical physics model), a rough estimate of the robot's pose and posture can be obtained.estimated value, and then use a filtering algorithm to combine this rough estimate with the actual sensor (such as GPS, gyroscope)MeasurementsFusion can obtain accurate robot position and attitudeestimated value。

7.2 Low pass filter

A filter that can easily pass low-frequency filtering is called a low-pass filter.

8. Mobile model

The position information of the robot at the last moment is given, and a control command is given, and the expected position of the robot after executing the command is calculated through a series of formulas. The series of formula calculations here are mobile models. Before implementing the filtering algorithm, it is necessary to model the robot's movement model first, that is, to establish the robot's movement model. There are two commonly used robot mobility models, namely:Mileage-based motion model，Speed-based mobility model。

Mileage-based robot mobility model

The taxi you usually take has an odometer. This odometer can count how far the tires have rotated. To understand intuitively, mileage is the amount of change in a certain indicator. The change can be a change in distance or an angle.

The mileage-based robot movement model (Odometry-based Motion Model), the problem to be solved is that the position and orientation angle of the robot at the previous moment are known, and the expected moving distance and angle of the robot during the period from the previous moment to the present moment are known In the case of changes, find the position and orientation angle of the robot at the current moment. For example, it is known that the position coordinates of the robot at the last moment are (1, 0), and the change during this period is (0.1, 0.2), the mobile model will calculate the estimated position of the robot at the current moment: (1, 0) + (0.1, 0.2 ) + (a random variable, a random variable).

9. Control model

Input the control command and the robot's position information, and the control model tells the robot how far it is expected to go and how much its body orientation changes after executing the command.

10. State estimation

10.1 Problem introduction

Take a simple example to describe the state estimation problem.

In a one-dimensional space, there is a coordinate $y_0$sign board. We stay put and use a rangefinder to measure the distance to the sign as $d$ . So, our current coordinatesxxWhat is$x\; ?$

The coordinates of the known signboard are $y_0$, as long as the distance dd to the sign is measured$d$ , we can get our coordinates$x=y_0-d$。

However, there is an error in our measuring instrument, and the calculation result is not accurate, so we can think about it this way: since we have not moved, we can measure it multiple times and take the average. At this point, we have completely constructed a state estimation problem:

• In a one-dimensional space, estimate your own coordinates $x$；
• Give a solution: Since the coordinates $y_0$, which can be used as a reference;
• Use a measuring instrument to measure the distance dd from yourself to the sign$d$ , use this to back-calculate your own coordinates$x$。

Let's call it "estimation" because we can't have infinite time to make infinite measurements to obtain coordinates $the\; true\; value\; of\; x$ . But when the number of measurements is enough, the result is considered accurate enough, that is, some means are used to approximate the true value.

10.2 Concepts related to state estimation

The above example can explain what is the state estimation of a discrete-time, linear, time-invariant, Gaussian system. Specifically, it can be explained as:

• Discrete time (discrete-time) : describes that our measurement method is not continuous in time;

• Linear : Describes the type of mathematical model of the sensor (rangefinder). Since it is an equation related to the observation of a reference object, we also call it: observation equation/observation model (observation model) . By reference $y$ and the ranging result$d$ , calculate its own coordinates$x$，即 $x=f(y,d)=y-d$ , the model isa linear function;

• Time-invariant : The mapping relationship in the sensor mathematical model does not change with time. Time-invariant does not refer to the state to be estimated $x$ does not change with time, it refers to the mapping relationship fwith the state$f:x,y\to d$ does not change with time. Simply put, this means thatthe observation equation does not change with time. For example: observation equation$f(x,y)=y-x$ will not become$f(x,y)=y-2x$ and other mapping relationships;

• Gaussian distribution : describes the type of measurement noise. Measurement $n$ obeys Gaussian distribution.

  Considering that there is an error in the rangefinder, that is, there is measurement noise, and the measured value $dactual$ is the result of combining the true value with measurement noise. Therefore, it is necessary to improve the measurement equation and add a measurement noise nnon the basis of the theoretical results$n$ . The formula looks like this:
  $$d=\underset{\text{theoretical result}=d_{true}}{\underbrace{f(x,y)}}+$$
  An ideal situation for$$n\; is\; that\; the\; measurement\; noise\; n\; is\; Gaussian\; distributed.$$

10.3 Measurements based on the law of large numbers

Measurement noise that follows a Gaussian distribution is a random variable, and when repeated measurements are made, the results obtained are not the same and fluctuate around the true value. But in the measurement number $When\; K$ is large enough, the accumulation of such noise items will tend to the mean. If the mean of the noise is 0, then the exact result can be approximately obtained:
$$\begin{array}{rl}{d}_{mean}& =\frac{1}{K}\sum d_k\\ & =d_{true}+\underset{\approx 0}{\underbrace{\frac{1}{K}\sum _{\approx 0}{n}_k}}\\ & \approx d_{true}\end{array}$$

3. Macroscopic understanding of Kalman filtering

Kalman filter series 1——Kalman filter

The Kalman filtering algorithm is a recursive predictive filtering algorithm. The algorithm involves filtering and also involves the prediction of data at the next moment.

1. Introduction of Kalman filter

Let's take a small example in life to introduce Kalman filtering: There is a coin and a ruler. The purpose is to obtain the size of the coin as accurately as possible. A total of k times of measurement are measured. Each measurement result is recorded as Z 1 $Z_1,Z_2,\dots Z_k$. How do we estimate the size of a coin? A natural idea is to take the average. The calculation process is as follows:

A simple understanding is that the estimated result of a certain measurement is related to the estimated result of the previous time, which embodies the recursive idea of ​​Kalman filtering. Expressed as:

Current estimated value = last estimated value + coefficient * (current measured value - last estimated value)

$${\hat{\mathrm{X}}}_k={\hat{X}}_{k-1}+\frac{1}{k}(Z_k-{\hat{X}}_{k-1})$$

Parameter explanation

• ${\hat{X}}_k$represents the kth estimated value
• $Z_k$Represents the k-th measurement value;
• ${\hat{X}}_{k-1}$Represents the k-1th estimated value;

It can be seen from the above formula that,kth estimated value = k-1th estimated value + $\frac{1}{k}$(kth measured value - k-1th estimated value). If k increases with the number of measurements, $\frac{1}{k}$will tend to 0, and the above formula will become: ${\hat{X}}_k={\hat{X}}_{k-1}$, that is, as k increases, the k-th estimated value is only related to the (k-1)-th estimated value, and has nothing to do with the k-th measured value. That is, as k increases, the measurement results are not important. In fact, this is also very easy to understand. When we measure many times, the previous estimated value is actually very close to the true value, so the significance of measuring one more time becomes less significant; on the contrary, if k is very small, that is, k =1 (k represents the number of measurements, k=0 is meaningless), the above formula will be: ${\hat{X}}_k=Z_k$, that is, the estimated value at this time is completely determined by the measured value.

2. Kalman coefficient

Convert 1 k \frac1k in the above formula$\frac{1}{k}$Replaced by ${\text{K}}_k$Indicates that ${\text{K}}_k$that isKalman coefficient。

${\text{K}}_k$The calculation formula is as shown in the figure below:

Macroscopic understanding of this formula:

• 当$e_{}\gg e_{AND}$When , that is, when the estimation error is much larger than the measurement error, ${\text{K}}_k$tends to 1, ${\hat{\mathrm{X}}}_k$tends to $Z_k$, indicating belief in the measured value.
• 当$e_{}\ll e_{AND}$When , that is, when the estimation error is much smaller than the measurement error, ${\text{K}}_k$tends to 0, ${\hat{X}}_k$tends to ${\hat{X}}_{k-1}$, indicating confidence in the estimate.

3. Kalman filter data fusion

This chapter uses a simple example to demonstrate the role of Kalman filter data fusion.

There is an electronic scale and an ordinary scale, and an object is measured at the same time. The weight measured by the electronic scale is Z1=30, and the standard deviation is $p_1=2$ ; The weight result measured by ordinary scales is Z2=32, and the standard deviation is$p_1=4$。

Assuming that they satisfy the normal distribution, you can draw the distribution diagram of the weight of the objects measured by the two scales, as follows:

The above picture Z1 shows the distribution of the measurement results of the electronic scale, and Z2 shows the distribution of the measurement results of the ordinary scale. Before doing the calculation, I think we can guess the real value first. It can be seen from the above that the standard deviation of electronic scales is only 2, while the standard deviation of ordinary scales is 4, which means that the measurement results of electronic scales are more stable, so the measurement results may be more accurate, and finally our fusion results may be more biased towards electronic scales. The result is closer to 30. Of course, these are just my guesses. Now let’s calculate and see if the optimal estimate is consistent with our guesses.

Taking the first measurement result Z1 as the estimated value and the second measurement result Z2 as the measured value, we can get the estimated value $\stackrel{The\; expression\; of\; ^}{\mathrm{Z}}$ is as follows:
$$\hat{Z}=Z_1+k\left(Z_2-Z_1\right)$$
in order to make$\hat{Z}$ Optimal, you need to make$Z$The standard deviation of$\stackrel{\sigma \; Z\; ^}{}$$p_{\hat{Z}}$Minimum, that is, variance $vyr\left(\hat{Z}\right)$is the minimum, and the derivation formula is as follows:

Given the infinitive: ${p_{\hat{Z}}}^2={\left(1-\mathrm{k}\right)}^2{p_1}^2+k^2{p_2}^2$ , in order to make${p_{\hat{Z}}}^2$ is the smallest, you need to find k. Let the derivation of the above formula with respect to k equal to 0 and find the extreme point. The derivation is as shown in the figure below:

So far we have the finalestimated valueandstandard deviation, we can see that this result is closer to 30 as we guessed before, and we can find that the standard deviation of this result is smaller than the standard deviation of the previous two measurements. Draw the distribution diagram of the three results, as shown below:

after_merge represents the result after fusion. As can be seen from the above figure, the distribution of the result after fusion is higher and thinner, and the effect is better.

4. Popular understanding of Kalman filtering

How to explain Kalman filtering in simple terms and in as much detail as possible?

Suppose you raise a pig:

One week ago, the pig's weight was 46±0.5kg. Note that I used ±0.5 here, which means that I am not so sure about the weight of this pig a week ago, that is to say, the weight of 46kg has an error of 0.5kg.

Now, I've had it for another week. So I want to know how much it will weigh in one week, and what is the approximate error?

In order to get the weight after one week, I have two methods: one is to calculate an approximate value based on the pig weight formula obtained from my many years of pig raising experience, and the other is to directly weigh it. Of course, both methods have certain errors. Assume that the weight obtained by the empirical formula is 48kg, with an error of 2kg; the weight obtained by directly weighing is 49kg, with an error of 1kg.

However, I am a Virgo, and I feel that whether it is the value obtained by the empirical formula or the value obtained by direct weighing, it is not accurate enough. I hope there is a method that can combine the weight of the pig a week ago, the value estimated by the empirical formula and the value obtained by direct weighing, and comprehensively consider to obtain a value that is closest to the real weight of the pig and has the smallest error. This is what Kalman filtering does.

Now let’s abstract the pig-raising model into a mathematical formula:

On the left side of the figure above, the weight of the pig in the previous week can be abstracted as the state value at time k-1, represented by the optimal estimated value at time k-1 plus an error term, and the same is true for the right side. Where,
$${\mathcal{P}}_{\mathrm{k}}=\mathbb{E}\left[{\mathbf{e}}_k{\mathbf{e}}_{\mathrm{k}}^{\mathrm{T}}\right]$$
This term represents the covariance of the estimated value. Here are two points:

1. All variables in the above figure are in bold, indicating that they are a vector or a matrix;
2. The reason why a (column) vector is used instead of a number to represent the state value is because although the weight of a pig can be represented by a value, many states cannot be represented by a number in practical applications (such as The missile's position in space has three coordinates: x, y, and z).

The right side of the figure above represents the state value at time k, which can be passed throughprediction module(i.e. estimating the weight of the pig based on an empirical formula) anderror correction module(That is, directly weigh the pig's weight) to estimate. Similarly, the prediction module and the error correction module have correspondingerrorandError covariance matrix. What Kalman filtering does isAccording to the relevant theory of Bayesian estimation, while considering the covariance of the prediction module and the error correction module, a larger weight is given to the item with a small error, a smaller weight is given to the item with a large error, and the prediction error is minimized.。

The specific implementation process is as follows:

4. Introduction to Kalman filtering

1 Introduction

Kalman filtering is described by a series of recursive mathematical formulas, which provide an efficient and computable method to estimate the state of the process and minimize the estimated mean square error. The Kalman filter is widely used and powerful and can estimate the past state, current state, and future state of a signal .

Kalman Filter (KF) is aOptimization Estimation Algorithm(Optimal Estimation Algorithm), commonly used in drones, automatic driving, satellite navigation, computer vision, signal processing and other fields, including robot navigation control systems, guidance systems, sensor data fusion systems, and military radar systems, missile tracking systems wait. The actual functions are mainly:Update predictions based on sensor measurements to achieve more accurate estimates。

2. Applicable scenarios

The Kalman filter is suitable for estimating a random variableThe optimal state of a dynamic system. Even if the observed system state parameters contain noise and the observation values ​​are inaccurate, Kalman filtering can still complete the optimal estimate of the true value of the state.

The essence of the Kalman filter algorithm is to iterate using the characteristic that the fusion of two normal distributions is still a normal distribution.

3. State Observer

State Observers: Used to optimize and estimate some states that cannot be measured directly but can be measured indirectly.

Scenario: Observing the internal temperature of a rocket injector

Now there is a rocket flying from the earth to Mars. The rocket uses liquid hydrogen as fuel, but excessive temperature will cause damage to the mechanical parts of the rocket injector. Therefore, it is very important to monitor the internal temperature of the injector. However, the sensor does not support placement in the injector. inside, otherwise it will melt away. But we can place a temperature sensor outside the injector to indirectly measure the internal temperature . But there is an error between the indirect measurement value and the real value. For this reason, we can use the state observer toEstimate the true value based on indirect measurements。

4. Best state estimator

Optimal State Estimator: Estimates the optimal system state from error-affected sensor measurements.

The Kalman filter itself is an optimal state estimator, which is similar to the state observer mentioned above. The difference is that KF is designed for stochastic systems. The predicted value formula of the state observer and the best state estimator (KF) is as follows:
$$\text{State observer}{\hat{x}}_{k+1}=A{\hat{x}}_k+B{u}_k+K({and}_k-C{\hat{x}}_k)\text{Deterministic system}$$

$$\text{Kalman filter}{\hat{x}}_k=A{\hat{x}}_{k-1}+B{u}_k+K_k(y_k-C(A{\hat{x}}_{k-1}+B{u}_k))\text{Stochastic system}$$

Scenario: Estimating the position of a car in a tunnel

If a car enters a tunnel, it will be difficult for the GPS receiver on the car to receive satellite signals. If you are driving on a street blocked by tall buildings, the positioning noise will be very large due to the multipath effect (signal distortion). We can simply think of this as a case where sensor measurements are affected by errors. If we want to estimate the true position of the car, it is recommended to use Kalman filtering.

The goal of KF is to combine the GPS measurement value (Measurement) and the mathematical model prediction value (Prediction) to find the optimal estimated state of the car position (ie, the car position).

5. Kalman gain

For example, the distance between the robot and the obstacle measured by lidar is 7m, and the accuracy is 90%. Based on the speed, the distance between the robot and the obstacle is estimated to be 6m, and the accuracy is 80%. The final distance estimation result is: result = ( 1
$$result=(1-\frac{0.9}{0.8+0.9})\ast 6+\frac{0.9}{0.8+0.9}\ast 7=6.52\text{meters}.$$
Among them,$\frac{0.9}{0.8+0.9}$known asKalman gain, representing this sensor data relative to an estimate calculated from speedReliability。

6. Other Kalman filters

Kalman filter is linear, but in practice, many data models are nonlinear. At this time, you can consider extended Kalman filter. In addition, interested friends can also learn about unscented Kalman filter and particle filter, as shown in Figure 12 Show.

Five, Kalman filter library

simdkalman

simdkalman

Python implements Kalman filtering

[Easy to understand] How to understand the Kalman filter algorithm that sent Chang'e to heaven?

Assuming that the missile is moving horizontally, the speed of the missile at each moment is dv, the standard deviation of the speed measuring instrument is v_std, and the position_noise of the missile measured by the GPS sensor at each moment and the variance of the GPS are predict_var.

# -*- coding: utf-8 -*-

import numpy as np


t = np.linspace(1,100,100) # 在1~100s内采样100次
a = 0.5 # 加速度值
position = (a * t**2)/2

position_noise = position+np.random.normal(0,120,size=(t.shape[0])) # 模拟生成GPS位置测量数据（带噪声）
import matplotlib.pyplot as plt
plt.plot(t,position,label='truth position')
plt.plot(t,position_noise,label='only use measured position')


#---------------卡尔曼滤波----------------
# 初始的估计导弹的位置就直接用GPS测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0
odo_var = 120**2 #这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低
v_std = 50 # 测量仪器的方差（这个方差在现实生活中是需要我们进行传感器标定才能算出来的，可搜Allan方差标定）
for i in range(1,t.shape[0]):
  
    dv =  (position[i]-position[i-1]) + np.random.normal(0,50) # 模拟从IMU读取出的速度
    position_predict = position_predict + dv # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std**2 # 更新预测数据的方差
    # 下面是Kalman滤波
    position_predict = position_predict*odo_var/(predict_var + odo_var)+position_noise[i]*predict_var/(predict_var + odo_var)
    # 高斯分布乘法公式
    predict_var = (predict_var * odo_var)/(predict_var + odo_var)
    predicts.append(position_predict)

    
plt.plot(t,predicts,label='kalman filtered position')

plt.legend()
plt.show()