(prerequisite-Kalman Filter) State Equation, Measurement Equation

• State Equation

 

 

Indicates the relationship between the previous state (ex. position) and the current state.

---

It breaks down as follows.

 

---

It can group as follows.

 

---

• Measurement Equation

 

 

Indicates the relationship between the current state and the sensor data.

---

It breaks down as follows.

 

---

1. Decomposition of State Equation

 

• The positional state

 

 

$x(k)$ : the position of the object in the k-th step.

$\stackrel{\dot{}<!--\; \dot{}\; -->}{x}(k)$ : the velocity of the object in the k-th step.

$\stackrel{¨<!--\; ¨\; -->}{x}(k)$ : the acceleration of the object in the k-th step.

$T$ : time interval between each step.

---

• Distance

 

 

$$\mathrm{Velocity\; *\; Time}$$ $$T\stackrel{\dot{}<!--\; \dot{}\; -->}{x}(k-<!--\; -\; -->1)$$

---

• Control Input

 

Extra Input Acceleration.

 

$u(k)$

---

• Noise of Control Input

 

$w(k)$

---

• Velocity

 

Velocity = Current Velocity + Acceleration * Time + Extra Input Acceleration + Control Input noise

---

• Acceleration

 

---

• Acceleration of previous step

 

$$\mathrm{1/2\; *\; Acceleration\; *\; Time^2}$$ $$\frac{T^2}{2}\stackrel{¨<!--\; ¨\; -->}{x}(k-<!--\; -\; -->1)$$

---

• State Matrix VS Control Matrix

 

 

You can group the previous equations as above.

A Matrix : State Matrix, for state modeling.

B Matrix : Control Matrix, for defining the control input.

---

2. Decomposition of Measurement Equation

 

 

$z(k)$ : Sensor data pointing to location
$v(k)$ : Sensor noise(Distribution)

---

• Distribution

 

Small Distribution VS Large Distribution

 

If the sensor noise is small (Small Distribution) : it is superimposed on a narroa area.

If the sensor noise is large (Large Distribution) : the area where the sensor recognizes the object appears very wide. It becomes difficult to accurately locate the object.

Because of this noise, it cannot always be $z(k)=x(k)$ and an error occurs.

---

• Transition Matrix

 

 

C Matrix : Transition Matrix

Match the three dimensions (Position, Velocity, Acceleration) in the state estimation equation to one dimension (position) of the sensor data.

 

https://gaussian37.github.io/autodrive-ose-state_equation/