// File: /workspace/github_c_makefile_star2_O0/Openvario[P]sensord/KalmanFilter1d.c
// Function: KalmanFiler1d_update
void KalmanFiler1d_update(t_kalmanfilter1d* filter, float z_abs, float var_z_abs, float dt)
{
	float F1=1.0;
	float dt2, dt3, dt4;
	float y;
	float s_inv;
	float k_abs;
	float k_vel;

	// check if dt is positive

	//Predict step
	//update state estimate
	filter->x_abs_ += filter->x_vel_ * dt;

	// update state covariance
	dt2 = dt * dt;
	dt3 = dt * dt2;
	dt4 = dt2 * dt2;

	filter->p_abs_abs_ += 2*(dt*filter->p_abs_vel_) + dt2 * filter->p_vel_vel_ + 0.25*(filter->var_x_accel_ * dt4);
  filter->p_abs_vel_ += dt * filter->p_vel_vel_ + (filter->var_x_accel_ * dt3)/2;
  filter->p_vel_vel_ += filter->var_x_accel_ * dt2;

	// Update step
	y = z_abs - filter->x_abs_;		// Innovation
	s_inv = F1 / (filter->p_abs_abs_ + var_z_abs);		// Innovation precision
	k_abs = filter->p_abs_abs_*s_inv; // Kalman gain
	k_vel = filter->p_abs_vel_*s_inv;

	// Update state estimate.
	filter->x_abs_ += k_abs * y;
	filter->x_vel_ += k_vel * y;

  // Update state covariance.
  filter->p_vel_vel_ -= filter->p_abs_vel_ * k_vel;
  filter->p_abs_vel_ -= filter->p_abs_vel_ * k_abs;
  filter->p_abs_abs_ -= filter->p_abs_abs_ * k_abs;

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
movq %rdi, -0x8(%rbp)
movss %xmm0, -0xc(%rbp)
movss %xmm1, -0x10(%rbp)
movss %xmm2, -0x14(%rbp)
movss 0x4f29(%rip), %xmm0     # 0xb5f8
movss %xmm0, -0x18(%rbp)
movq -0x8(%rbp), %rax
movss 0x4(%rax), %xmm0
movss -0x14(%rbp), %xmm2
movq -0x8(%rbp), %rax
movss (%rax), %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, (%rax)
movss -0x14(%rbp), %xmm0
mulss -0x14(%rbp), %xmm0
movss %xmm0, -0x1c(%rbp)
movss -0x14(%rbp), %xmm0
mulss -0x1c(%rbp), %xmm0
movss %xmm0, -0x20(%rbp)
movss -0x1c(%rbp), %xmm0
mulss -0x1c(%rbp), %xmm0
movss %xmm0, -0x24(%rbp)
movss -0x14(%rbp), %xmm0
movq -0x8(%rbp), %rax
mulss 0xc(%rax), %xmm0
movss -0x1c(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulss 0x10(%rax), %xmm1
addss %xmm0, %xmm0
addss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movq -0x8(%rbp), %rax
movss 0x14(%rax), %xmm1
mulss -0x24(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
movsd 0x5933(%rip), %xmm2     # 0xc098
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movq -0x8(%rbp), %rax
movss 0x8(%rax), %xmm0
cvtss2sd %xmm0, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x8(%rax)
movss -0x14(%rbp), %xmm0
movq -0x8(%rbp), %rax
movss 0x10(%rax), %xmm2
movq -0x8(%rbp), %rax
movss 0x14(%rax), %xmm1
mulss -0x20(%rbp), %xmm1
movss 0x58e5(%rip), %xmm3     # 0xc090
divss %xmm3, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movq -0x8(%rbp), %rax
addss 0xc(%rax), %xmm0
movss %xmm0, 0xc(%rax)
movq -0x8(%rbp), %rax
movss 0x14(%rax), %xmm0
movss -0x1c(%rbp), %xmm2
movq -0x8(%rbp), %rax
movss 0x10(%rax), %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, 0x10(%rax)
movss -0xc(%rbp), %xmm0
movq -0x8(%rbp), %rax
subss (%rax), %xmm0
movss %xmm0, -0x28(%rbp)
movss -0x18(%rbp), %xmm0
movq -0x8(%rbp), %rax
movss 0x8(%rax), %xmm1
addss -0x10(%rbp), %xmm1
divss %xmm1, %xmm0
movss %xmm0, -0x2c(%rbp)
movq -0x8(%rbp), %rax
movss 0x8(%rax), %xmm0
mulss -0x2c(%rbp), %xmm0
movss %xmm0, -0x30(%rbp)
movq -0x8(%rbp), %rax
movss 0xc(%rax), %xmm0
mulss -0x2c(%rbp), %xmm0
movss %xmm0, -0x34(%rbp)
movss -0x30(%rbp), %xmm0
movss -0x28(%rbp), %xmm2
movq -0x8(%rbp), %rax
movss (%rax), %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, (%rax)
movss -0x34(%rbp), %xmm0
movss -0x28(%rbp), %xmm2
movq -0x8(%rbp), %rax
movss 0x4(%rax), %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, 0x4(%rax)
movq -0x8(%rbp), %rax
movss 0xc(%rax), %xmm0
movss -0x34(%rbp), %xmm2
movq -0x8(%rbp), %rax
movss 0x10(%rax), %xmm1
movd %xmm0, %ecx
xorl $0x80000000, %ecx       # imm = 0x80000000
movd %ecx, %xmm0
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, 0x10(%rax)
movq -0x8(%rbp), %rax
movss 0xc(%rax), %xmm0
movss -0x30(%rbp), %xmm2
movq -0x8(%rbp), %rax
movss 0xc(%rax), %xmm1
movd %xmm0, %ecx
xorl $0x80000000, %ecx       # imm = 0x80000000
movd %ecx, %xmm0
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, 0xc(%rax)
movq -0x8(%rbp), %rax
movss 0x8(%rax), %xmm0
movss -0x30(%rbp), %xmm2
movq -0x8(%rbp), %rax
movss 0x8(%rax), %xmm1
movd %xmm0, %ecx
xorl $0x80000000, %ecx       # imm = 0x80000000
movd %ecx, %xmm0
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, 0x8(%rax)
popq %rbp
retq
nopw %cs:(%rax,%rax)