luweikxy
(Luweikxy)
February 2, 2024, 1:34am
1
```
```
if (!new_value) {
// we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss
// of our yaw source
_omega_yaw_P *= 0.97f;
return;
}
// convert the error vector to body frame
const float error_z = _dcm_matrix.c.z * yaw_error;
// the spin rate changes the P gain, and disables the
// integration at higher rates
const float spin_rate = _omega.length();
// sanity check _kp_yaw
if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
_kp_yaw.set(AP_AHRS_YAW_P_MIN);
}

Why not use 3 axis of yaw_error?
i.e.

```
Vector3f error = Vector3f(0,0, yaw_error);
// convert the error vector to body frame
error = _dcm_matrix.mul_transpose(error);
// ...
// _omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw.get();
_omega_yaw_P = error * _P_gain(spin_rate) * _kp_yaw.get();
```

?
This seems to lack a suitable reason to do so.

At here, I see that at begining, DCM code use 3 axis of yaw_error, but at this issue:

committed 05:11AM - 06 Jul 12 UTC

the new correction algorithm copes poorly with gyro drift, leading to
significat… ion attitude errors in the face of drift

, it was fixed to only use z axis of error with no clear explanation.

kd0aij
(Mark Whitehorn)
February 2, 2024, 3:29pm
3
Here is the paper referenced in that comment:

luweikxy
(Luweikxy)
February 5, 2024, 2:20am
4
I had post an issue, but was told that post questions on the discuss forums:

opened 07:46AM - 01 Feb 24 UTC

closed 08:40AM - 01 Feb 24 UTC

@priseborough @tridge
https://github.com/ArduPilot/ardupilot/blob/master/lib… raries/AP_AHRS/AP_AHRS_DCM.cpp#L576
![image](https://github.com/ArduPilot/ardupilot/assets/19629731/f60065ef-0664-411d-83a7-61f35589c521)
Why not use 3 axis of yaw_error?
i.e,
```
Vector3f error = Vector3f(0,0, yaw_error);
// convert the error vector to body frame
error = _dcm_matrix.mul_transpose(error);
// ...
// _omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw.get();
_omega_yaw_P = error * _P_gain(spin_rate) * _kp_yaw.get();
```
?
This seems to lack a suitable reason to do so.
At here, I see that at begining, DCM code use 3 axis of yaw_error, but at this issue:
https://github.com/ArduPilot/ardupilot/commit/302696951a49da26d81ad4748d6aab1cdb7c84f2
![image](https://github.com/ArduPilot/ardupilot/assets/19629731/78a68772-071f-4e53-b826-8216387349ee)
, it was fixed to only use z axis of error with no clear explanation.

. So I go here.

luweikxy
(Luweikxy)
February 5, 2024, 2:27am
5
Many thanks!

I have read this paper after you told me, I guess the reason is magnetometer has latency, so should not use magnetometer to fix x and y axis in body fame? I am not sure…

BTW, this paper is hard to read, for example, I can not figure out this formula:
this formula above is some like the SO(3) matrix update formula in complementary filter:
But I can not derive it(equation 1).

kd0aij
(Mark Whitehorn)
February 5, 2024, 4:23am
6
He says equation 1 is derived from his DCM algorithm which is described in this paper:

It probably requires a fair amount of mathematical skill to duplicate his results…

luweikxy
(Luweikxy)
February 5, 2024, 7:37am
7
I have read this paper several times, but it still hasn’t helped me in any way to understand equation (1)…

If only there were more mathematical derivation details…