From theory to practical - how to understand that the Chang-e into space Kalman filter algorithm Kalman filter?

Original link: https: //zhuanlan.zhihu.com/p/77327349

Intuitive understanding

** What first Kalman filter to solve the problem? ** I have a fixed location to launch military missile attack enemy targets, for example (out of the total points feelings of technology, always talking about what sports car, these thermometers too low). Missile need to open every second distance to the target in the radar measurement. Because there is an error then the radar, the information on the need to integrate their time position, velocity, etc. to more accurate determination of the target distance from the present time . This is the Kalman filter needs to resolve the matter

From an intuitive understanding of the Kalman filter is how to solve this issue?

First of all known missile "This second current radar measurements of the distance from the target missile (we call it the observations, such as radar to directly measure the distance from the target missile 7m )," "the time the missile away from the target distance" and "their current missile the speed of time, "the three data. According to "the distance from the target missile in time" and "current speed missile own time" we can estimate the distance (we call it the target of missiles from the estimated value ). For example: one second on goal from 10m, speed is 4m / s, so now this second estimate on the distance from the target is 6M . The speed data can be read from the engine that can be obtained from the sensor inside. (Depending on the speed estimated the missile from the mobile computing is called modeling, want to understand the link between the model and the observations and mobile filtering algorithm can see the article "Mobile Robot model: predicted robot position according to the control command")

So the question is, the distance from the target missile now both observations 7m, there are a estimated value of 6m. I believe in the end what? Simply believe that the observations in case the enemy radar is interfered with it? I believe that in case of a simple estimate of the distance in time or speed estimates are not allowed to do? So, we want to get the final estimate of the distance from the target missile based on the accuracy of observations and estimates. High accuracy on the final result of the high proportion of low accuracy on low proportion. 7m if the radar measurement accuracy of 49%, according to the speed of 6m estimated that 1% accuracy, then the final result is that the distance estimation
(intuitive understanding of these data we have like, end the method of calculation will be given)

Kalman filter how to do?

We first reviewed at an intuitive understanding of how to do it.

The velocity and position of the one second missile missile estimated current position of the missile rough estimate of time.
The measured position of the missile and radar measurements we calculate a rough estimate of the position of the missile to a weighted linear and accurate position estimate missile according to both data reliability.
We also mentioned earlier in the position of the missile and radar measurements are erroneous. So Kalman want to use probability to measure the reliability of the data.

For example: the measurement of radar data it is not just a number of. But that measure the probability of detecting a missile 7m 0.8 in that position, there is a probability that position at 7.2m 0.1. 0.1 in 6.9m probability that position. This data is called the probability distribution. Meaning that the probability distribution of the probability of how much data as well as their respective appear composed of a number of values ​​is called the probability distribution.

Kalman believe speed missile, missile position, radar ranging measurements which are normally distributed (for that is the normal high school). This is what does it mean? For example, the following chart is about the probability distribution of the position of the missile in time, seen from the graph so it is the biggest in the highest probability ordinate 10m (horizontal axis) at that location. It is the probability of relatively little elsewhere.

Kalman Filtering Python code Practice

This practice is known amount of horizontal motion missiles, missile known standard deviation for each time the speed dv, and speed measuring instruments is v_std, also known measured each time the missile position position_noise variance with GPS and GPS is predict_var. (Note that the standard deviation is the square of the variance, do not be surprised)
we need to get location information position_predict after the integration of two sensor Kalman filter based on this information

#coding=utf-8

import numpy as np
#-----------------------------生成无误差的GPS坐标点------------------------------------------
# 模拟数据
t = np.linspace(1,100,100)#在1-100之间，按照同样的间隔生成100个数据
a = 0.5 #加速度
position = (a * t**2)/2 #根据加速度获取位置，模拟GPS获取的位置
#给位置添加一个具有正态分布的误差
# random.normal 正态分布 均值，标准差。样本数（t.shape[0]获取和第一维度数一样的样本数）
#---------------------------生成带高斯分布误差的GPS测量值误差--------------------------
position_noise = position+np.random.normal(0,120,size=(t.shape[0]))
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 # 速度标准差
for i in range(1,t.shape[0]):#循环从1开始到样本数前一个结束
  
    dv =  (position[i]-position[i-1]) + np.random.normal(0,50) # 模拟从IMU读取出的速度
    #------------------预测--------------------
    position_predict = position_predict + dv # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std**2 # 预测速度方差
    # ---------------更新------------------------
    #根据权重更新位置预测
    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)**2#更新速度方差
    predicts.append(position_predict)

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

plt.legend()
plt.show()