Discrete-Time Integrator Compare

Perform discrete-time integration or accumulation of signalexpand

https://www.mathworks.com/help/simulink/slref/discretetimeintegrator.html

$$\begin{gathered} \text{gain} \\ K=1 \end{gathered}$$

Forward Euler Method

$$\begin{array}{}\text{(1)}& x(n+1)& =& x(n)+K\ast T\ast u(n)\text{(2)}& y(n)& =& x(n)\end{array}$$

find the

$y(n)$

---

$$\underline{\begin{array}{rlrlr}y(n+1)& =& x(n+1)& & \text{from }(2)\\ y(n+1)& =& x(n)+K\ast T\ast u(n+1)& & \text{from }(1)\\ y(n+1)& =& y(n)+K\ast T\ast u(n+1)& & \text{substitute }x(n)\text{ from }(2)\\ y(n)& =& y(n-1)+K\ast T\ast u(n)& & \text{make }u(n)\text{ is now}\end{array}}$$

tag_angle_integral = tag_angle_integral + tag_angle * update_rate_;

Trapezoidal Method

$$\begin{array}{}\text{(3)}& x(n)& =& y(n-1)+K\ast T/2\ast u(n-1)\text{(4)}& y(n)& =& x(n)+K\ast T/2\ast u(n)\end{array}$$

find

$y(n)$

---

$$\overline{)\begin{array}{rlrlr}y(n)& =& y(n-1)+K\ast T/2\ast u(n-1)+K\ast T/2\ast u(n)& & \text{substitute }(4)\text{ into }x(n)\end{array}}$$

tag_angle_integral =    tag_angle_integral + 
                        last_tag_angle * 0.5 * update_rate_ + 
                        tag_angle * 0.5 * update_rate_;

反正微分都會多浪費一個記憶體了，就用Trapezoidal Method 吧